Apparatus and method for determining representation of environment

Abstract

Apparatus and methods for determining a representation of an environment are disclosed. Aspects of the present disclosure relate to a control system for a vehicle, the control system including at least one controller, the control system configured to: receive sensor data indicative of a position of one or more objects detected in an environment of the vehicle from at least one sensor; and modifying an occupancy grid stored in a memory accessible to the control system as a function of the sensor data, the occupancy grid representing an environment of the vehicle and having a plurality of cells wherein each cell is associated with at least one value indicative of a likelihood that a corresponding portion of the environment is occupied by one of the one or more objects, wherein the modifying comprises the control system configured to: determine one or more cells occupying the grid in which a field of view of at least one sensor is occluded by at least one of the one or more objects; and modifying at least one value associated with the determined one or more cells occupying the grid to indicate an increase in uncertainty in which the cell is occupied by the object.

Description

Technical Field

[0001] The present disclosure relates to determining an occupancy grid representing an environment. Aspects of the invention relate to control systems, systems, vehicles, methods, and computer software. Background Art

[0002] A robot must determine information about its environment, such as information about objects in the environment. In cases where the robot is capable of autonomous movement, such as an autonomous vehicle or a vehicle with at least partial autonomous capabilities, information about the objects is used for navigation to avoid the objects. It is well known that occupancy grids are used to store information about the environment, such as the locations of objects in the environment. The occupancy grid represents the environment of the robot and has a plurality of cells, each cell representing a part of the environment and storing an indication of the occupancy probability of the corresponding part. Sensors associated with the robot (such as a vehicle) provide measurement data indicating the locations of the objects, and the measurement data is used to update the occupancy grid. In this way, the occupancy grid can be used to track dynamic objects in the environment.

[0003] Typically, the sensors associated with the robot are high-definition sensors that provide dense measurement data related to the environment, such as dense point cloud data indicating the objects. An example of such a high-definition sensor is a 64-lidar sensor. Due to this high density, it is also possible to reliably infer the absence of objects, such as empty spaces. However, such high-definition or high-density sensors have associated costs and complexities. There is a desire to use low-definition or low-density sensors to update the occupancy grid.

[0004] The aim of the present invention is to solve one or more drawbacks associated with the prior art. Summary of the Invention

[0005] Aspects and embodiments of the present invention provide control systems, systems, vehicles, methods, and computer software as claimed in the appended claims.

[0006] According to one aspect of the present invention, there is provided a control system for a vehicle, the control system including at least one controller, the control system being configured to: receive sensor data indicating the position of one or more objects detected in the environment of the vehicle from at least one sensor; modify an occupancy grid stored in a memory accessible to the control system based on the sensor data, wherein the modification includes the control system being configured to determine one or more cells of the occupancy grid that are within the field of view of the sensor for which the sensor data does not indicate the position of an object. Advantageous knowledge that a cell is within the field of view of the sensor means that if an object is present, it is likely to have been detected. The control system may modify the value associated with the determined one or more cells of the occupancy grid for which the sensor data does not indicate the position of an object.

[0007] According to one aspect of the present invention, there is provided a control system for a vehicle, the control system including at least one controller, the control system being configured to: receive sensor data indicating the position of one or more objects detected in the environment of the vehicle from at least one sensor; modify an occupancy grid stored in a memory accessible to the control system based on the sensor data, wherein the modification includes the control system being configured to: determine one or more cells of the occupancy grid that are within the field of view of the sensor for which the sensor data does not indicate the position of an object; and modify the value associated with the determined one or more cells of the occupancy grid to indicate an increased uncertainty associated with the determined one or more cells being occupied. Advantageously, the occupancy grid indicates cells corresponding to positions in the environment for which the uncertainty of occupancy is increased due to a portion of the environment being occluded.

[0008] According to one aspect of the present invention, there is provided a control system for a robot, the control system including at least one controller, the control system being configured to: receive sensor data indicating the position of one or more objects detected in the environment of the vehicle from at least one sensor; and determine one or more cells of the occupancy grid in which the field of view of the sensor is occluded by at least one of the one or more objects; and modify at least one value associated with the determined one or more cells of the occupancy grid to indicate an increased uncertainty that the cell is occupied by an object. Advantageously, the occupancy grid indicates cells corresponding to positions in the environment for which the uncertainty of occupancy is increased due to a portion of the environment being occluded.

[0009] According to one aspect of the present invention, there is provided a control system for a vehicle. The control system includes at least one controller and is configured to: receive sensor data from at least one sensor indicating the position of one or more objects detected in the environment of the vehicle; modify an occupancy grid stored in a memory accessible to the control system according to the sensor data, the occupancy grid representing the environment of the vehicle and having a plurality of cells, wherein each cell is associated with at least one value indicating the likelihood that the corresponding portion of the environment is occupied by one of the one or more objects, and wherein the modification includes the control system being configured to: determine one or more cells of the occupancy grid that are within the field of view of the sensor and for which the sensor data does not indicate the position of an object; and modify the at least one value associated with the determined one or more cells of the occupancy grid to indicate an increased likelihood that the cells are not occupied by an object. Advantageously, the occupancy grid indicates cells corresponding to positions in the environment for which, due to a portion of the environment being within the field of view of the sensor, the likelihood that the cells are not occupied is increased, but the sensor data does not indicate the position of an object.

[0010] According to one aspect of the present invention, there is provided a control system for a vehicle. The control system includes at least one controller and is configured to: receive sensor data from at least one sensor indicating the position of one or more objects detected in the environment of the vehicle; modify an occupancy grid stored in a memory accessible to the control system according to the sensor data, the occupancy grid representing the environment of the vehicle and having a plurality of cells, wherein each cell is associated with at least one value indicating the likelihood that the corresponding portion of the environment is occupied by one of the one or more objects, and wherein the modification includes the control system being configured to: determine one or more cells of the occupancy grid that are within the field of view of the sensor and for which the sensor data does not indicate the position of an object; and modify the at least one value associated with the determined one or more cells of the occupancy grid to indicate an increased likelihood that the cells are not occupied by an object. Advantageously, the occupancy grid indicates cells corresponding to positions in the environment for which, due to a portion of the environment being within the field of view of the sensor, the likelihood that the cells are not occupied is increased, but the sensor data does not indicate the position of an object.

[0011] One or more cells of the occupancy grid that are within the field of view of the sensor may be assumed to be empty cells for which the sensor data does not indicate the position of an object. Advantageously, since the sensor data does not indicate the position of an object corresponding to the cell, the cell is assumed to be empty.

[0012] The modification optionally includes the control system being configured to: determine one or more cells of the occupancy grid in which the field of view of the sensor is occluded by at least one of the one or more objects; and modify at least one value associated with the determined one or more cells of the occupancy grid to indicate an increased uncertainty that the cell is occupied by an object. Advantageously, due to a portion of the environment being occluded by the sensor, at least one value is modified.

[0013] At least one of the one or more objects may have a position corresponding to at least one other cell of the occupancy grid. Advantageously, the position of the object is associated with at least one other cell of the occupancy grid. The one or more cells in which the field of view of the sensor is occluded may be occluded cells. Advantageously, the occluded cells are identified as cells within the field of view, but the sensor cannot determine the position of the object. The one or more cells in which the field of view of the sensor is occluded by at least one of the one or more objects are determined to be in the shadow of one of the one or more objects detected in the environment of the vehicle. Advantageously, the sensor may not be able to observe the cells in the shadow.

[0014] The modification optionally includes the control system combining a predetermined free quality value with a prior free quality for one or more cells of the occupancy grid that are within the field of view of the sensor, for which the sensor data does not indicate the position of an object. Advantageously, the combination introduces an updated value into the prior free quality.

[0015] The predetermined free quality value may be equal to or greater than 0.7. The free quality value may be equal to or greater than 0.9. Advantageously, the free quality value may be a value that promotes the cell to indicate unoccupied, i.e., having a relatively high free quality.

[0016] The modification may include the control system combining a predetermined unknown quality value with a prior unknown quality for one or more cells of the occupancy grid that are within the field of view of the sensor, for which the sensor data does not indicate the position of an object. Advantageously, the unknown quality value may indicate that the occupancy of the cell is unknown.

[0017] The predetermined unknown quality value may be equal to or greater than 0.4. The unknown quality value may be approximately 0.5. Advantageously, the unknown quality value may indicate that the occupancy of the cell is unknown.

[0018] Optionally, setting the unknown quality includes setting the free quality value and the occupancy quality value associated with each cell to corresponding values such that the free quality is equal to or greater than a predetermined value. The unknown quality value may correspond to the difference between the free quality value and the occupancy quality value associated with each cell. Advantageously, the free quality value and the occupancy quality value can be controlled.

[0019] The combination may optionally be performed according to the Dempster Shafer combination rule. Advantageously, the Dempster Shafer combination rule allows the combination of a predetermined value and a previous value.

[0020] The modification may include that the control system is configured to: determine the field of view of the sensor according to one or more characteristics associated with the sensor; and select the cells of the occupancy grid corresponding to the field of view of the sensor. Advantageously, the field of view is determined according to the characteristics to be similar to the actual field of view of the sensor.

[0021] The modification may optionally include that the control system is configured to: determine a first field of view of the sensor; determine one or more cells of the occupancy grid within the first field of view of the sensor, for which the sensor data does not indicate the position of an object; determine a first cell of the occupancy grid within the first field of view of the sensor, which first cell corresponds to the position of an object among one or more objects detected in the environment of the vehicle; and determine a second field of view of the sensor according to one or more characteristics associated with the sensor and the position of an object among one or more objects. Advantageously, the field of view of each corresponding sensor is determined.

[0022] The control system may optionally be configured to determine the cells of the occupancy grid, in which the field of view of the sensor is blocked by at least one object among one or more objects, as the cells in the shadow of an object among one or more objects detected in the environment of the vehicle. Advantageously, the field of view does not extend to the area in the shadow, and thus the sensor cannot accurately determine the occupancy of such cells.

[0023] The control system may be configured to sequentially select the cells of the occupancy grid outward from the position of the sensor to determine whether the sensor data indicates the position of an object. Advantageously, the outward processing of the cells allows for efficient modification of the cell values.

[0024] According to another aspect of the present invention, there is provided a system for a vehicle, the system comprising: the control system according to any one of the preceding claims; and at least one sensor, the at least one sensor being arranged to output sensor data indicating the position of one or more objects detected in the environment of the vehicle.

[0025] The at least one sensor may include one or more of a radar sensor, a lidar sensor, and / or an optical sensor.

[0026] According to one aspect of the present invention, there is provided a vehicle comprising the control system as described above or the system as described above.

[0027] According to a further aspect of the present invention, there is provided a computer-implemented method, comprising: receiving sensor data from at least one sensor indicating the position of one or more objects detected in the environment of a vehicle; modifying an occupancy grid stored in a memory based on the sensor data, the occupancy grid representing the environment of the vehicle and having a plurality of cells, wherein each cell is associated with at least one value indicating the likelihood that a corresponding portion of the environment is occupied by one of the one or more objects, wherein the modification comprises: determining one or more cells of the occupancy grid that are within the field of view of the sensor for which the sensor data does not indicate the position of an object; and modifying the at least one value associated with the determined one or more cells of the occupancy grid to indicate an increased likelihood that the cells are not occupied by an object.

[0028] The at least one value associated with the determined one or more cells of the occupancy grid may be an unknown quality value. The method may include modifying the unknown quality value to a value indicating the uncertainty that the cell is occupied by an object. The unknown quality value may be assigned a value between 0.4 and 0.6.

[0029] The at least one value associated with the determined one or more cells of the occupancy grid may be a free quality value and an occupancy quality value. The method may include modifying the free quality value and the occupancy quality value to respective values indicating the uncertainty that the cell is occupied by an object.

[0030] The modification optionally comprises: determining one or more cells of the occupancy grid that are within the field of view of the sensor for which the sensor data does not indicate the position of an object; and modifying the at least one value associated with the determined one or more cells of the occupancy grid to indicate an increased likelihood that the cells are occupied by an object.

[0031] The modification optionally comprises: determining one or more cells of the occupancy grid in which the field of view of the sensor is occluded by at least one of the one or more objects; and modifying the at least one value associated with the determined one or more cells of the occupancy grid to indicate an increased uncertainty that the cell is occupied by an object.

[0032] The modification optionally comprises combining a predetermined free quality value with a prior free quality for one or more cells of the occupancy grid that are within the field of view of the sensor for which the sensor data does not indicate the position of an object.

[0033] The predetermined free quality value may be equal to or greater than 0.7.

[0034] The modification may include combining a predetermined unknown mass value of one or more cells of an occupancy grid that are within the field of view of a sensor with a previous unknown mass, for which sensor data does not indicate the location of an object.

[0035] The combination may be performed according to Dempster Shafer combination rules.

[0036] The modification optionally includes: determining the field of view of the sensor according to one or more characteristics associated with the sensor; and selecting one or more cells of the occupancy grid corresponding to the field of view of the sensor.

[0037] The modification optionally includes: determining a first field of view of the sensor; determining one or more cells of the occupancy grid that are within the first field of view of the sensor, for which sensor data does not indicate the location of an object; determining a first cell of the occupancy grid that is within the first field of view of the sensor and that corresponds to the location of one of one or more objects detected in the environment of the vehicle; and determining a second field of view of the sensor according to one or more characteristics associated with the sensor and the location of one of one or more objects.

[0038] According to a further aspect of the invention, there is provided a computer software which, when executed by a computer, is arranged to perform the method as described above. A computer-readable data storage medium may tangibly store the computer software thereon.

[0039] Within the scope of the present application, it is expressly intended that the various aspects, embodiments, examples and alternatives and in particular the respective features set forth in the preceding paragraphs, in the claims and / or in the following description and drawings may be adopted independently or in any combination. That is to say, the features of all embodiments and / or any embodiment may be combined in any way and / or combination, unless such features are incompatible. The applicant reserves the right to change any originally filed claim or to file any new claim accordingly, including the right to modify any originally filed claim to be dependent on any other claim and / or to incorporate any feature of any other claim, even though originally not claimed in this way. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] One or more embodiments of the invention will now be described, by way of example only, with reference to the accompanying drawings, in which:

[0041] Figure 1 A system according to an embodiment of the invention is shown;

[0042] Figure 2 A vehicle according to an embodiment of the invention is shown;

[0043] Figure 3 Shows an illustration of an occupancy grid according to an embodiment of the present invention;

[0044] Figure 4 Shows a method according to an embodiment of the present invention;

[0045] Figure 5 Shows a method according to another embodiment of the present invention;

[0046] Figure 6 Shows an illustration of an occupancy grid according to another embodiment of the present invention; and

[0047] Figure 7 Shows a probability according to an embodiment of the present invention. Detailed Description

[0048] Referring to Figure 1 , shows a system 100 according to an embodiment of the present invention. As Figure 2 shown, according to an embodiment of the present invention, the system 100 is used with a robot such as a vehicle 200. The vehicle 200 is an example of a robot having the ability to autonomously move or navigate to navigate in the environment in which the vehicle 200 is located. For example, the vehicle 200 may have a level 4 or 5 autonomy as defined by SAE International. The vehicle 200 may include the system 100 as part of providing such an ability. Although the embodiments of the present invention are described with respect to such a vehicle 200, it should be understood that the embodiments of the present invention are not limited to this aspect.

[0049] The system 100 is arranged to determine an occupancy grid 300 related to the environment of the vehicle 200. The occupancy grid 300 represents the environment of the vehicle 200 and has a plurality of cells, each cell representing a part of the environment and associated with a probability indicating the occupancy of the corresponding part corresponding to the cell. It should be understood that the occupancy grid is useful for navigating the vehicle 200 in the environment, for example, for tracking the movement of one or more objects in the environment.

[0050] Figure 3 Shows an example representation of an occupancy grid or occupancy grid map 300. The occupancy grid map 300 and the cells forming part of the occupancy grid map 300 are defined by parameters that define their dimensions, such that the size and resolution of the occupancy grid map 300 can be selected for each specific use case. In Figure 3 the example, the size of the occupancy grid map 300 is 8×8 and is thus formed by 64 cells. The occupancy grid map 300 is selected to have a cell size of 1 m×1 m, and thus the example square occupancy grid map 300 has sides of length 8 m and represents 64 m2 The environmental area. It should be understood that the shown occupancy grid map 300 is relatively small for practical applications, and a larger occupancy grid map is usually used, such as an occupancy grid map of 120m×120m, for example, but the embodiments of the present invention are not limited to this aspect. The occupancy grid map 300 is formed by rows and columns that can each have a corresponding number of cells. In some embodiments, each cell has a linear cell index as shown, and for a cell in a corner, such as the upper left corner of the occupancy grid map 300 shown, this linear cell index can start from 0. In some embodiments, it is assumed that the vehicle 200 is located at the center of the occupancy grid 300, as indicated by the shown arrow. In such an embodiment, a relative coordinate space system can be used, where the cells in front of the vehicle are assigned positive x coordinate values, and the cells to the left of the vehicle are associated with positive y coordinate values. Such a coordinate space can be referred to as an EGO relative coordinate space.

[0051] Figure 1 The system according to an embodiment of the present invention shown is arranged to output data indicating the occupancy grid map 300 for navigating the vehicle 200. As will be described, the occupancy grid map 300 is determined based on sensor measurement data indicating the environment of the vehicle 200, which is provided by one or more sensors associated with the vehicle 200. At least some of the one or more sensors can provide sensor measurement data indicating the detection of an object whose position corresponds to one or more cells in the occupancy grid map 300 (an object can be determined to correspond to more than one cell). In an embodiment of the present invention, the field of view (FOV) of at least one of the sensors is determined, and for any cell within the FOV of the sensor where the sensor measurement data does not indicate the position of an object, the data associated with that cell is updated or modified to indicate an increased likelihood that the cell is not occupied by an object. This enables a robust occupancy grid map 300 to be determined even when the sensor measurement data is sparse or of low resolution. Using the FOV of the sensor in the process of determining the occupancy grid map 300 is intended to ensure that a portion of the environment within the visible range of the sensor is determined as free space.

[0052] Figure 1 The shown system 100 includes one or more controllers 110. In the example shown, the system 100 includes one controller 110, but it should be understood that the embodiments of the present invention are not limited to this aspect. The controller 110 is arranged to output data 155 indicating the occupancy grid map 300 in use.

[0053] Each controller 110 may include respective processing means 120, such as an electronic processing means 120 or a computer processor, hereinafter referred to as processor 120. The processor 120 is arranged to execute, in operation, computer-readable instructions that may be stored in a memory means 130 formed by one or more memory devices 130, which memory means 130 forms a memory 130 communicatively coupled to the processing means 120. The controller 120 includes an input device 140 and an output device 150. The input device 140 may include an electrical input 140 of the controller 110. The output device 150 may include an electrical output 150 of the controller 110. In some embodiments, the input device 140 and the output device 150 may be unified, for example, in the form of a network interface, which input device 140 and output device 150 input and output data to and from a communication bus of the vehicle 200, respectively. Thus, the controller 110 may receive data from the communication bus and output data to the communication bus. In some embodiments, the input device 140 is arranged to receive sensor measurement data 145 from one or more sensors 160, 170 associated with the vehicle 200. In an example, the system 100 includes a first sensor 160 and a second sensor 170, but it should be understood that this is merely an example. At least some of the sensors 160, 170 may emit radiation from a laser, for example, and receive the radiation reflected from an object, for example, via a photodiode or the like. The reflected radiation is used to determine points corresponding to the object, where each set of points corresponding to the reflection of the radiation forms a point cloud. Data indicating the detection of the object is provided to the controller 110 as sensor measurement data 145.

[0054] The processor 120 is arranged to store data indicating an occupancy grid map 300 in the memory 130. The processor 120 is arranged to update or modify the occupancy grid map 300 based on incoming sensor measurement data 145. The fusion of the incoming sensor measurement data with the occupancy grid may be performed by dynamic occupancy grid map (DOGMa) processing. The DOGMa processing 300 may be performed at periodic time intervals based on the received sensor measurement data 145. A time interval variable may be used to represent the time between iterations or cycles of the DOGMa processing as illustrated in the following binding box 410. The DOGMa processing provides a method for fusing sensor data in a dynamic occupancy grid map. More details about the DOGMa processing are provided by D. Nuss in the Ph.D. thesis “A Random Finite Set Approach for Dynamic Occupancy Grid Maps”, which was also published in the International Journal of Robotics Research in 2018 and is incorporated herein by reference.

[0055] Embodiments of the present invention are described in connection with DOGMa processing. However, it should be understood that other processing, such as using direct associations of cells to track horizontal fusion or Bayesian occupancy filters, can be used to update or modify the occupancy grid, and the usefulness of the embodiments of the present invention is not limited to DOGMa processing. Embodiments of the present invention operate based on the field of view of the sensor providing the sensor data to affect the values associated with at least some of the cells of the occupancy grid. In particular, as will be described, embodiments of the present invention affect the probabilities associated with at least some of the free cells. In some embodiments, when at least some of the cells in the occupancy grid are occluded and not within the field of view of the sensor, the values associated with these cells are affected to indicate the uncertainty associated with the cell state. In this way, not only are the values associated with the cells whose positions correspond to the detected objects updated, but also the values associated with the cells assumed to be free or empty and the occluded cells are updated. Advantageously, this improves the update or modification of the occupancy grid and can allow, for example, the use of sensors with lower resolution, thereby being able to reduce costs, for example.

[0056] In some embodiments, a particle filter is used to model the state of the occupancy grid map 300. The particle filter maintains a list of particle states, one or more weights, and cell indices, as well as the cell indices of the occupancy grid map 300 to which the particles belong. A label can be assigned to each particle as an identifier of the object (if any) to which the particle belongs. As will be described, the next state of the particle is predicted and updated by the detection of an object in the sensor measurement data 145.

[0057] Referring to Figure 4 A method 400 according to an embodiment of the present invention will be described. The method 400 can be executed by the processor 120, and the instructions representing the method can be stored in the memory 130 as computer-readable instructions.

[0058] A prediction operation is performed in block 410. The prediction operation predicts the new positions of the particles and accordingly updates the cells of the occupancy grid map 300 associated with the particles.

[0059] In some embodiments, for the purpose of the prediction operation in block 410, a transformation matrix is created using the time_step variable, which indicates the periodic time interval between the periods of the method 400 for updating or modifying the occupancy grid map 300. A noise generation function can be used to create a process_noise matrix for modeling the uncertainty in particle prediction. This can be performed by using a randomly generated matrix with a configurable, i.e., selected, standard deviation.

[0060] In the particle prediction of block 410, the state of the particles used for DOGMa processing is predicted. The state can be represented by position and velocity, e.g., x, y, vx, vy. For reasons of computational complexity, in some embodiments, a two-dimensional world is used where the z-axis is not modeled. The state prediction can be performed by matrix multiplication between the current state and a transition matrix. In some embodiments, a process_noise matrix is added to the result of the multiplication to introduce uncertainty. For some particles near the edge of the occupancy grid map 300, when the velocity of these particles would cause them to cross the boundary in the next time step, these particles are predicted to leave the range of the occupancy grid map 300. These particles are removed by the DOGMa processing.

[0061] Whenever a prediction is performed in block 410, the weight of each particle may be reduced. This reduction can be performed by the multiplication of a weight matrix and a persistence_probability variable. The weight matrix is a matrix that can be used in some embodiments to store the weights associated with each particle. The weight matrix can be a 1×N matrix or vector, where N is the number of particles. The persistence_probability is a parameter that controls the lifespan of the particles in the DOGMa processing. The persistence_probability can have a value between 0.0 and 1.0, where 0.0 means the particle will not persist and 1.0 means the particle weight will not be reduced. Thus, a value between 0 and 1 is typically chosen. As a result of the prediction operation in block 410, each particle has an updated position and velocity. Since the particles have moved, the cell indices to which the particles are assigned are updated. The update of the cell indices can be performed by mapping the x, y state indicating the position of each particle to the position of the cell of the occupancy grid map 300 to which the particle belongs.

[0062] As those skilled in the art will understand, cell occupancy can use the basic belief assignment (BBA) from Dempster-Schafer theory. At the basic level of DOGMa processing, this means that variables can be maintained as follows: predicted_occupancy_mass, which represents the likelihood that a cell in the occupancy grid map 300 is occupied; and predicted_free_mass, which represents the likelihood that a cell in the occupancy grid map 300 is unoccupied or free. Here, in block 410, for each cell in the occupancy grid map 300, these masses can be determined using the particle weights assigned to each corresponding cell (e.g., the weights stored in the weight matrix). The BBA is then stored as a prior mass or prior BBA, which is subsequently used to determine the mass or posterior BBA for posterior calculations, as will be described. In other words, the prior BBA is updated before receiving measurements, and the posterior BBA is updated during the iterations of method 400.

[0063] In block 420, one or more sensors 160, 170 associated with the vehicle 200 detect any objects in the environment of the vehicle 200. Each sensor 160, 170 outputs sensor measurement data 145 indicative of any detected object, where the sensor measurement data 145 is received by the controller 110. In some embodiments, the sensor measurement data 145 is formed as a measurement grid map as discussed below. In some embodiments, the measurement grid map is another grid map that can match the dimensions of the DOGMa occupancy grid map 300, i.e., in this example, the measurement grid map 300 can be an 8×8 cell as Figure 3 shown, but it should be understood that measurement grid maps of other sizes can be used.

[0064] In block 430, the detection of any object indicated by the sensor measurement data 145 received in block 420 is mapped onto the measurement grid map in block 420 and assigned to the cells of the measurement grid map. As discussed above, for at least some of the sensors 160, 170, the detection of an object in the sensor measurement data 145 is provided as a point cloud. The point cloud includes point data indicating the positions of the objects detected by the sensors 160, 170. The points in the point cloud have associated x, y, z localizations. In particular, for a land robot or vehicle, the occupancy grid map 300 can be 2D, i.e., lacking the z dimension. Thus, the points in the point cloud can be "flattened". As part of the flattening, points with a z-axis value that are outside the region of interest associated with the vehicle may be removed. For example, points detected 20 meters above the EGO vehicle do not form part of the measurement grid map 300. Then, in block 430, the flattened points are mapped or assigned to the cells of the measurement grid map.

[0065] In block 440, a cell occupancy probability is determined for the cells of the measurement grid map. Each cell of the measurement grid map is associated with one or both of an occupancy mass and a free mass. In some embodiments, each cell of the measurement grid map, as well as each cell of the occupancy grid map, is associated with a value indicating the occupancy mass and a value indicating the free mass of the corresponding cell. The occupancy mass is the probability or confidence that the cell is occupied by an object, while the free mass is the probability that the cell is free or unoccupied. Based on these mass values, a value indicating an unknown mass or probability can be determined as the difference between the two mass values.

[0066] In some embodiments, the detection of an object in a cell increases the occupancy mass of one or more adjacent cells. In some embodiments, a 2D Gaussian kernel is created such that the detection in one cell of the measured grid map can improve the occupancy mass of adjacent cells. The Gaussian kernel generates a matrix, where the values within the cells of the matrix correspond to a Gaussian distribution with a mean of 0 and a standard deviation given by the sigma parameter. The occupancy mass of each cell of the measured grid map can be initialized to all zeros, and then a confidence that each cell is occupied is generated using a list of cells that contain detections from box 420. In some embodiments, the confidence can be generated by using matrix convolution of the Gaussian kernel applied to the measured grid map. A vector of occupancy masses is produced, which is used to generate the cell occupancy BBA for the occupancy grid map as described above. When the cell mass is updated or modified multiple times, such as when a cell is adjacent to two detections or when there are multiple detections in a cell, the Dempster-Schafer combination rule can be used. As a result of box 440, in some embodiments, each cell of the measured grid is associated with variables occupancy_mass and free_mass, where the variable occupancy_mass represents the likelihood that the corresponding cell of the measured grid map 300 is occupied, and the variable free_mass represents the likelihood that the corresponding cell is empty or free. In other embodiments, each cell can be associated with a data structure such as occupancy_bba that contains values corresponding to the values of occupancy_mass and free_mass.

[0067] In box 450, the FOV BBA combined with the BBA of the measured grid map determined in box 440 is determined. In box 450, for each of at least one of the one or more sensors 160, 170, the quality of the cells is affected by the FOV of the sensor. In particular, in some embodiments, in cells within the FOV of sensors 160, 170 where no object is detected, i.e., cells where no detected object exists, the quality of the cells is determined to indicate free. As will be described, in some embodiments, for cells within the FOV but occluded by an object and not visible to sensors 160, 170, since they are not visible to sensors 160, 170, their quality is determined to have a quality indicating unknown occupancy because the occupancy of the cells cannot be determined.

[0068] Figure 5 A method 500 of applying the FOV of sensors 160, 170 to a measured grid map according to an embodiment of the present invention is shown. Method 500 can be performed in box 450. Reference will be made to Figure 6Method 500 is described. A measurement grid map 600 of sensors 160, 170 associated with a vehicle located at the origin 0, 0 (lower left corner) is shown. Thus, Figure 6 The shown ground Figure 1 part is located in the upper right of vehicle 200. The measurement grid map 600 shows, with diagonal shading, the cells corresponding to the detection of an object, such as cell 640. As described above, method 500 is described in conjunction with Nuss's DOGMa processing, where it should be understood that its usefulness is not limited to DOGMa processing.

[0069] Referring Figure 5 , method 500 includes block 510 that determines a frame of cells of the measurement grid within the FOV of the sensors. In the example, the FOV of the sensors is described as conical. It will be understood that in other examples, the FOV of sensors 160, 170 may have other shapes, such as circular, elliptical, or rectangular. Figure 6 The FOVs of sensors 160, 170 are shown as a conical shape 605 extending between a first boundary line 610 and a second boundary line 620, where the first boundary line 610 and the second boundary line 620 are located on each peripheral side of the conical shape 605 extending from the origin (0, 0) where sensors 160, 170 are located. The FOV 605 of sensors 160, 170 may be referred to as the first FOV 605.

[0070] In this embodiment, using the extrinsic parameters or characteristics of each sensor 160, 170, such as one or more of its position around the vehicle, angular resolution, and range, a FOV, conical shape 605, is generated for each sensor 160, 170. The conical shape 605 represents the region that is empty with a high confidence, except for the detection of an object and the occluded space behind the detection, as will be described. For the FoV of each sensor 160, 170, method 500 determines the FOV starting from the position of the sensor 160, 170 relative to the center point of the EGO vehicle, such as the conical shape 605 in the example, and in some embodiments, the center point of the EGO vehicle may be the rear axle.

[0071] In some embodiments of method 500, the environment around the EGO vehicle is divided into multiple regions or sectors. In one embodiment, the environment is divided into octants, but it should be recognized that other numbers of sectors may be used. Then, method 500 can be executed separately for each sector, which advantageously reduces the computational complexity. In some embodiments, the method can be executed at least partially in parallel for two or more sectors, which reduces the computation time. The FOV of each sensor 160, 170, such as the conical shape 605 projected by the sensor, can intersect multiple octants, i.e., the FOV of the sensor does not have to be limited to one sector. InFigure 6 In [the figure], in block 510, a cone 605 is projected onto a measurement grid map 600, which has been divided into cells of uniform size.

[0072] In block 520, at least a portion of the measurement grid map 600 that is within the FOV 605 of the sensor is selected, i.e., one or more cells within the cone 605 in the illustrated embodiment. If a cell is only partially within the FOV 605 of the sensor, the cell can be selected. In some embodiments, method 500 moves from the origin (0, 0) of the cone 605 based on the angular resolution and range of the corresponding sensors of sensors 160, 170, traversing the measurement grid map 600 column by column until the boundary of the cone is reached. In this example, the ranges of sensors 160, 170 are considered to extend to the periphery of the measurement grid map 600, but it should be understood that the ranges can fall within the measurement grid map 600, i.e., they may not reach the edges of the map 600. For example, in block 520, the first column 630 is selected, and one or more cells in that column that are at least partially between the boundaries 610, 620 are selected. In an embodiment, the first column is the column with the lowest column index.

[0073] In block 530, it is determined whether the selected cell contains a detection of an object, and if not, method 500 moves to block 540.

[0074] In block 540, for cells that are at least partially within the FOV 605 of sensors 160, 170 and are not occupied by an object, these cells are marked as visible to sensors 160, 170 and are empty or idle, i.e., unoccupied. Advantageously, since only the cells within the FOV 605 of sensors 160, 170 are marked in this way, this ensures that only cells that are expected to have detected an object (within the FOV 605 of sensors 160, 170) but have not actually detected an object are marked, such as cell 615. Thus, for cell 615, the sensor data does not indicate the location of an object in the cell 615, where the cell is within the FOV 605. Therefore, when other cells within the FOV 605 have corresponding sensor data indicating a detection of an object and thus demonstrate that sensors 160, 170 are operational, it can be better assumed that cell 615 is actually empty because no object has been detected within cell 615.

[0075] If, in block 530, one of the selected cells, i.e., the cells in the current column, contains a detection of an object, method 500 is recursive because it returns to block 510, where a new FOV is projected for sensors 160, 170. In a second iteration of block 510, method 500 creates a new FOV between one or more detected objects and / or at the boundaries of the previous FOV. For example, cell 640, which is determined in block 530 to include an object detection, causes the method to return to block 510. In block 510, a second FOV 650 is projected that includes the boundaries 660, 670 between the cells containing the object detection, i.e., cells 640, 645, or the boundaries of the current sector or octant, as Figure 6 shown. Method 500 then repeats for the newly projected FOV 650 of the sensors. The newly projected FOV 650 represents the region where sensors 160, 170 have visibility between the detected objects.

[0076] After marking the cells as visible and empty in block 540, in block 550, it is considered whether all cells visible to the sensors within the current FOVs 605, 650 of sensors 160, 170 have been considered. If not, the method returns to block 520 to select one or more additional cells, e.g., in the next column of the measurement grid map 600. However, if all visible cells have been considered, the method moves to block 560.

[0077] In block 560, the remaining cells that are not visible and are empty or do not correspond to the detected objects 640, 645 within the FOVs 605, 650 of sensors 160, 170 are marked as not visible to sensors 160, 170, or as occluded to sensors 160, 170, e.g., cell 680. For these cells 680, which are not visible to the sensors due to being occluded by object 640, sensors 160, 170 cannot determine whether these cells 680 correspond to the location of an object. Thus, the sensors do not know or are uncertain about the occupancy of these occluded cells 680.

[0078] As a result of method 500, the cells in the measurement grid map 600 that are not occupied by detected objects are marked or identified as visible and empty, e.g., cell 690, or as occluded, e.g., cell 680. It can be considered that illumination rays are projected outward from sensors 160, 170 to illuminate the regions within the FOVs 605, 650 of sensors 160, 170. The unoccupied illuminated cells of the measurement grid map 600 are considered empty 690, while the cells 680 within the shadow of object 640 caused by the illumination are considered to have uncertain occupancy.

[0079] For cells 690 determined to be visible and empty in block 540, the free mass of these cells is set to a predetermined value. The predetermined value indicates the probability of being free, for example at least 0.5. The predetermined free mass value can be, for example, 0.95, i.e., a 95% probability of being free, but it should be understood that other values can be selected. As can be understood, since these cells 690 are actually empty, i.e., within the FOVs 605, 650 of sensors 160, 170, and the certainty corresponding to other cells related to the detection of an object, i.e., cells 640, 645, is increased, it is possible to reliably infer that the sensor is operable. There can be an increased confidence that the visible and empty cells 690 do not correspond to an object, and thus the value associated with the cell can be controlled to indicate the increased confidence, for example, a predetermined free mass value equal to or greater than 0.7, or equal to or greater than 0.8, 0.9, or 0.95. Therefore, when the free mass value is combined with the prior free mass value of the cell, the cell is represented as empty more quickly.

[0080] In block 560, for occluded cells, an unknown mass or probability can be set to a predetermined value, for example 0.5, but other values can be selected. In some embodiments, the unknown mass value can be between 0.4 and 0.6. An unknown mass of 0.5 is equivalent to a 50:50 likelihood of being occupied and unoccupied. In some embodiments, the occupied_mass value and the free_mass value can be set to predetermined values, for example indicating a 50:50 likelihood. For example, the occupied_mass value can be set to 0.5, and the free_mass value can be set to 0.5. However, in other embodiments, both values can be set to 0 to indicate an unknown mass of 1. It will be understood that other values can be selected to achieve a similar or equivalent effect. For example, the free mass value and the occupied mass value can be updated to be less than 0.3 respectively.

[0081] Since the Dempster - Shafer combination rule is used to combine the unknown mass or BBA with the prior mass, as discussed below, the occupied mass gradually, i.e., after several iterations or updates, moves from the previously measured value to indicate an unknown occupancy. Thus, if a previously obtained detection is now in an occluded or blocked cell (e.g., Figure 6If it is in one of the units 680 (in the unit 680), the occupancy quality of the unit does not immediately change to indicate unknown occupancy. The occluded unit takes several cycles to reduce the occupancy quality to indicate unknown occupancy. The reverse is also true; if a previously visible and empty unit is now occluded, the unit takes multiple iterations or cycles to indicate unknown occupancy. In some embodiments, the amount of time or number of iterations required for this to occur can be controlled by a parameter fov_threshold (fov threshold), which controls which units are considered opaque by the algorithm, i.e., which occupancy qualities of the units are considered to be occupied by the detection. For example, if fov_threshold is set to 0.4, any unit with a fov_threshold value equal to or greater than 0.4 is considered to be occupied.

[0082] In block 470, the quality value determined to be the result of the FOV of the sensor is combined with the quality value of the measured grid map.

[0083] The combination of the FoV and the measured grid map can be performed using the Dempster - Shafer (DS) combination rule. Figure 7 The occupancy quality and free quality of two units 710, 720 are shown, and the occupancy quality and free quality will be combined using the DS combination rule as follows.

[0084] The first unit 710 has an occupancy quality equal to 0.2 (p(o1) = 0.2), a free quality equal to 0.३ (p(f1) = 0.३), and an unknown quality equal to 0.5. The second unit 720 has p(o2) = 0.5, p(f2) = 0.25, and an unknown quality equal to 0.25.

[0085] According to the DS formula, K is equal to the sum of the products of the combinations of all disjoint sets. In this example, for the sake of illustration, this means occupancy and free. These two possibilities are mutually exclusive and can therefore be considered "disjoint sets". This can be expressed as:

[0086] K = p(o1) * p(f2)+p(o2) * p(f1) = 0.2 * 0.25 + 0.5 * 0.3 = 0.2

[0087] K' = 1 / (1 - K) = 1.25

[0088] The uncertainty is intuitively calculated as:

[0089] p(u1) = 1 - p(o1) - p(f1) = 0.5

[0090] p(u2) = 1 - p(o2) - p(f2) = 0.25

[0091] The numerator is the sum of the products of all intersecting set combinations:

[0092] p(o3) = K′(p(o1) * p(o2) + p(o1) * p(u2) + p(o2) * p(u1))

[0093] = 1.25 * (0.2 * 0.5 + 0.2 * 0.25 + 0.5 * 0.5)

[0094] = 1.25 * 0.4 = 0.5

[0095] p(f3) = K′(p(f1) * p(f2) + p(f1) * p(u2) + p(f2) * p(u1))

[0096] = 1.25 * (0.3 * 0.25 + 0.3 * 0.25 + 0.25 * 0.5)

[0097] = 1.25 * 0.275 = 0.34375

[0098] p(u3) = 1 - p(o3) - p(f3) = 0.15625

[0099] In some embodiments, particle association is performed for detection in block 460. This allows DOGMa to process, create, and maintain particles associated with the detection. Advantageously, computational effort is not wasted on associated particles that do not model anything in the real world.

[0100] Return Figure 4 , in block 460 of method 400, one or more particles are associated with the measurement grid map. In some embodiments, the data structure particle_association_info (particle_association_information) is associated with the measurement grid map. This data structure stores, for each cell, the probability that the particles in that cell are associated with the measurement of an object. In some embodiments, the probability y is determined using the probability density function according to the normal Gaussian distribution, but it should be understood that other distributions can be used. The function can be:

[0101]

[0102] where y indicates the probability of the cell, and thus any particle within the cell associated with the measurement of an object, whose mean (m) can be 0, standard deviation (s) can be 1, and x is the distance in meters from the cell to the object detection. Thus, in block 460, for each particle in each cell, the probability y of the particle associated with the detection is determined.

[0103] In block 470, one or more components of a particle are determined. The particle for which one or more components are determined in block 470 can be a newborn particle and a persistent particle in the posterior confidence, i.e., the posterior BBA.

[0104] In some embodiments, one or more components, which can be represented as occupied_mass_components, are determined using occupied_mass from the determined measurement grid map, prior_occupied_mass (prior_occupied_mass) from the DOGMa grid determined in the prediction as in block 410, and the birth_probability parameter. Occupied_mass_components represent the components of the newborn particles used in the resampling block 480 below, and the components of the persistently existing particles used in the update block 475 below. The birth_factor parameter controls the balance between newly created particles and persistent particles. The birth_factor parameter can assume values within a predetermined range, for example, between 0 and 1. The higher the value, the less likely the particles are to persist between time steps, thus controlling the sensitivity of method 400 to incoming detections. A value of 0 means no new particles are created, while a value of 1 means particles will be created directly relative to the occupied mass. Creating a larger number of new particles results in pruning a larger number of existing particles in the sample block 480. As an example, the birth probability can be approximately 0.02, but other values can be chosen. The following function returns the set of persistent particles (pers_mass) used in the update block 475 below, and the set of particles to be created (born_mass) used in the example block 480.

[0105] scaled_tree_plausibility = birth_tactor * (1 - prior_occupied_mass)

[0106]

[0107] Note that occupied_mass and scaled_free_plausibility are vectors that are multiplied by the Schur product. The purpose of the scaled_free_plausability variable is to reduce the probability of particle birth.

[0108] where pers_mass can be calculated as follows:

[0109] pers_mass = occupied_mass - born_mass

[0110] The persistent mass pers_mass is the remainder obtained by subtracting the newborn mass from the occupancy mass. Note that the above operation is an element-wise vector operation.

[0111] Box 470 is an update block. In box 475, the cells and particles in the dynamic occupancy grid map are updated or modified using the detections incorporated into the measurement grid map, which also includes the effects of FOV determination according to the embodiments of the present invention discussed above with specific reference to Figure 5 the present invention.

[0112] For use in particle updates, a likelihood function can be defined, which calculates the Doppler measurement likelihood according to the method from the DOGMa paper (Nuss, 2017) (Equations 5.70 and 5.71 on page 77). Substantially, the update operation in box 475 is the determination of the likelihood of a particle persisting relative to the incoming measurements. That is, if a particle is close to the measurement and has a similar velocity, then that particle will be given a greater weight.

[0113] Box 475 can include cell occupancy and particle update processing. In box 475, the cell occupancy is updated by updating the posterior BBA using the BBA of the measurement grid map and the prior BBA. As described above, the Dempster-Schafer combination rule can be used for the occupancy mass and the free mass to perform the update.

[0114] In box 475, the particle weights are updated. In some embodiments, a function for updating the particle weights is used, which performs a vector multiplication between the existing, unnormalized weight of the particle and the Doppler measurement likelihood mentioned above. Updating the particles in this way reduces the particle weights according to the degree to which the state of the particle differs from the incoming detections.

[0115] In some embodiments, a normalization factor can be used for each cell to normalize the weights of the particles within each cell. This normalization is performed separately for associated (associated with the detection) and unassociated cells. For associated cells, the persistent mass of the cell can be divided by the sum of the particle weights in that cell. The persistent mass is related to the incoming detections in the measurement grid map, where the persistent mass is calculated in box 470 above. This is performed for each cell in the measurement grid map. In effect, this processing increases the particle weights of the particles that are at or near the incoming detections from sensors 160, 170. The unassociated cells can be updated by dividing the persistent mass by the prior occupancy mass. The unassociated and associated weight normalization factors are updated in this way. These are vectors with entries for each cell.

[0116] In some embodiments, the weight vector can be normalized. The normalization can use the unnormalised_weights of each particle, the associated_particles_normalisation_factors of each cell, the unassociated_particles_normalisation_factors of each cell, and the association_prob from the measured grid map. This step of block 475 determines the probability that the particles in the cell are associated with the measurement for each cell. In some embodiments, each particle has associated and unassociated probabilities that, when added together, can form the weight of the particle.

[0117] The associated part can be calculated by multiplying the association probability of the cell by the associated particle normalization factor and the unnormalized weight of the particle.

[0118] associated_part = association_prob * associated_particles_normalisation_factors(cell) * unnormalised_weights(particle)

[0119] The unassociated part is calculated as follows:

[0120] unassociated_part = (1 - association_prob) * unassociated_particles_normalisation_factors(cell) * weight(particle)

[0121] The above processing can be performed for each particle.

[0122] In block 480, a resampling operation is performed. During the resampling operation, new particles are initialized, which can be done according to the birth mass discussed above, and other existing particles are culled. As mentioned above, culling can be performed to keep the total number of particles in the dynamic grid map at the defined num_particles parameter, which provides the desired number of particles for the grid map. After the new particles are born, the total number of particles is num_particles + number_of_birth_particles, where number_of_birth_particles defines the number of newly born particles. Culling is used to reduce the number of particles to num_particles. In some embodiments, culling can include generating a number of uniformly distributed random numbers. The distribution of the random numbers will be num_particles to index the particles in the total number of particles. These are the particles that will persist, and the rest are culled, thus keeping the number of particles at num_particles.

[0123] In some embodiments, a sampling function is used. The sampling function is a function that aids in performing weighted random sampling. The use of the function is modular because different sampling functions can be used in block 480. In one embodiment, a discrete distribution is used, which returns a random number, where the probability of an integer I is defined as:

[0124]

[0125] where the weight w as the i-th integer i is divided by the sum S of all n weights, where n is the number of weights. It can be considered that the random sampler has a greater likelihood of retaining particles with higher weights.

[0126] New particles can be created during the resampling operation in block 480. In some embodiments, the creation of particles can be controlled by the parameter new_particles_per_cell: the number of new particles that should be created in each cell, which is proportional to the born_mass.

[0127] born_mass: The component of the posterior occupancy mass of new particles for each cell.

[0128] probability_for_cell: This contains the probability for each cell that the particles in the cell are associated with the measurement.

[0129] max_velocity: The maximum velocity to be used for newly initialized particles, in meters per second.

[0130] In some embodiments, a particle creation function is used that returns a pair of values, where the 'first' member is an N×4 matrix of the particle state as [x y vx vy], where N is the total number of new particles, and the'second' member is an N×1 vector of particle weights.

[0131] New particles can be instantiated in cells, where the higher the born_mass, the greater the number of particles that will be created. Since born_mass is related to the incoming detection, this results in more particles being created around the incoming detection. These particles are created with a positioning and velocity strategy. The strategy can include instantiating the particles using a distribution inside the cell (e.g., a standard random distribution). Advantageously, the particles are not all created at the center of the grid cell, but are randomly distributed around the cell with random velocities. In some embodiments, all new particles are unassociated, so they are instantiated with a label ID indicating unassociation (e.g., -1), but other labels can be used.

[0132] Creating new particles can provide a vector of new particles. The new particles can be attached to existing particles. In some embodiments, the created combined vector of particles is resampled, e.g., using the sampling function mentioned above. In this way, new particles for the particle filter are created, and at the same time, low-weight particles are removed so that the total number of particles remains within a defined limit.

[0133] In block 490, the labels associated with the particles are updated to represent objects. The DBSCAN algorithm (Ester et al., 1996) can be used to cluster the particles, but other methods for clustering the particles can also be used. The clustering creates clusters of particles with the same label, and the clusters of particles will be extracted as untracked objects with a center point, velocity, and bounding box. The algorithm associates groups of particles as objects. The configurable min_points determines the minimum number of particles that need to be in a cluster in order for it to be considered an object, and the ε variable is used to configure the allowed distance between particles that are considered part of a cluster.

[0134] This object extraction allows the dynamic occupancy grid to produce untracked objects, which are then output as additional sensor input to an object-level tracker (OLT) for fusion with data from other sensors, such as camera devices and radar sensors. Since these objects are rather simple, e.g., they do not have any classification details, they can be fused with data from other sensors, such as camera / radar, that allow for high-quality classification, and improve the position and velocity estimation of those sensors.

[0135] In some embodiments, a conversion from the BBA to a value representing the occupancy probability is performed. Advantageously, such a conversion facilitates the use of an occupancy grid map. The value representing the probability can be an integer, for example in the range from 0 to 1, rather than the BBA with two different possibilities (occupied and free). Thus, in some embodiments, the BBA is converted into a probability value for each cell to be occupied. This conversion can be performed by the following formula:

[0136] Probability = cell_occupancy + 0.5 × (1 - cell_occupancy - cell_free)

[0137] In block 495, the occupancy grid map is output. As described above, the occupancy grid map contains the occupancy probability of each cell in the occupancy grid map, as well as an associated list of detected objects extracted from the occupancy grid map. In block 495, the controller 110 outputs the occupancy grid map data 155 indicating the occupancy grid map 300 and the objects.

[0138] The occupancy grid map data 155 can be provided to another controller of the vehicle 200 for controlling the movement of the vehicle 200. The occupancy grid map data 155 can be used to determine the path of the vehicle 200. The occupancy grid map data 155 can be used to reduce the likelihood of the vehicle 200 coming into contact with any object in the environment of the vehicle 200.

[0139] The object can be used as an input to an object-level tracker (OLT) that performs association, tracking, and state estimation on the trajectories from multiple sensors. The OLT can perform the association using the Munkres algorithm (Konstantinova et al., 2003). The tracker maintains a list of trajectories updated with the association output. Finally, the state estimation can be processed by reusing the particle filter described above or using an unscented Kalman filter (UKF). A much smaller number of particles in the pairwise-down version is used to model the state of each trajectory. This is done by removing the grid. Instead, the world modeled by the particle filter is the bounding box of the trajectory, updated by any incoming detections that have been determined to be associated with that trajectory by the association step.

[0140] It should be understood that various changes and modifications can be made to the present invention without departing from the scope of the present application.

Claims

1. A control system for a vehicle, the control system including at least one controller, the control system being configured to: Receive sensor data from at least one sensor indicating the location of one or more objects detected in the environment of the vehicle; and modify an occupancy grid stored in a memory accessible to the control system based on the sensor data, the occupancy grid representing the environment of the vehicle and having a plurality of cells, wherein, each cell is associated with at least one value indicating the likelihood that a corresponding portion of the environment is occupied by one of the one or more objects, wherein the modification includes the control system being configured to: determine one or more cells of the occupancy grid in which the field of view of the at least one sensor is occluded by at least one of the one or more objects; and modify the at least one value associated with the determined one or more cells of the occupancy grid to indicate an increased uncertainty that the cell is occupied by an object.

2. The control system according to claim 1, wherein The at least one value associated with the determined one or more cells of the occupancy grid is an unknown quality value, and the control system is arranged to modify the unknown quality value to a value indicating an increased uncertainty that the cell is occupied by the object, and optionally, wherein the unknown quality value is assigned a value between 0.4 and 0.

6.

3. The control system according to claim 1, wherein, The at least one value associated with the determined one or more cells of the occupancy grid is a free quality value and an occupied quality value, and the control system is arranged to modify the free quality value and the occupied quality value to respective values indicating an increased uncertainty that the cell is occupied by the object, and optionally, wherein each of the free quality value and the occupied quality: is assigned a value between 0.4 and 0.6; is assigned a value less than 0.3; or is assigned a value substantially equal to 0.

4. The control system according to any one of the preceding claims, wherein, The modification includes the control system being configured to: determine one or more cells of the occupancy grid within the field of view of the sensor for which the sensor data does not indicate the location of an object; and modify the at least one value associated with the determined one or more cells of the occupancy grid to indicate an increased likelihood that the cell is not occupied by an object.

5. The control system according to any one of the preceding claims, wherein, The at least one value associated with the one or more cells of the occupancy grid for which the sensor data does not indicate the location of an object is a free quality value, and the control system is arranged to modify the free quality value to a value indicating the likelihood that the cell is not occupied by the object.

6. The control system according to claim 2 or any claim dependent on claim 2, wherein, The modification includes the control system combining a predetermined unknown quality value with a prior unknown quality for the one or more cells of the occupancy grid within the field of view of the sensor in which the field of view of the sensor is occluded by at least one of the one or more objects, and optionally, wherein the combination is performed according to Dempster Shafer combination rules.

7. The control system according to any one of the preceding claims, wherein, The modification includes the control system being configured to: determine the field of view of the sensor based on one or more characteristics associated with the sensor; and Select the one or more cells of the occupancy grid corresponding to the field of view of the sensor.

8. The control system according to any one of the preceding claims, wherein, The modification includes that the control system is configured to: Determine a first field of view of the sensor; Determine one or more cells of the occupancy grid within the first field of view of the sensor; Determine a cell of the occupancy grid within the first field of view of the sensor corresponding to the position of one of the one or more objects detected in the environment of the vehicle; And Determine a second field of view of the sensor based on one or more characteristics associated with the sensor and the position of one of the one or more objects.

9. The control system according to any one of the preceding claims, wherein, The control system is configured to: Determine the one or more cells of the occupancy grid in which the field of view of the sensor is blocked by at least one of the one or more objects as cells in the shadow of one of the one or more objects detected in the environment of the vehicle.

10. The control system according to any one of the preceding claims, wherein the control system is configured to sequentially select cells of the occupancy grid outward from the position of the sensor to determine whether the sensor data indicates the position of an object.

11. The control system according to any one of the preceding claims, wherein, The control system is arranged to divide the environment of the vehicle into a plurality of regions around the vehicle and update the occupancy grid separately for each of the plurality of regions.

12. A system for a vehicle, the system comprising: The control system according to any one of the preceding claims; And At least one sensor arranged to output sensor data indicative of the position of one or more objects detected in the environment of the vehicle.

13. A vehicle comprising the control system according to any one of claims 1 to 11 or the system according to claim 12.

14. A computer-implemented method, comprising: Receiving sensor data indicative of the position of one or more objects detected in the environment of a vehicle from at least one sensor; And Modifying an occupancy grid stored in a memory based on the sensor data, the occupancy grid representing the environment of the vehicle and having a plurality of cells, wherein each cell is associated with at least one value indicative of the likelihood that the corresponding portion of the environment is occupied by one of the one or more objects, wherein the modification includes that the control system is configured to: Determine one or more cells of the occupancy grid in which the field of view of the at least one sensor is blocked by at least one of the one or more objects; and Modify the at least one value associated with the determined one or more cells of the occupancy grid to indicate an increased uncertainty that the cell is occupied by an object.

15. A computer software which, when executed by a computer, is arranged to perform the method according to claim 14; optionally, the computer software is tangibly stored on a computer-readable medium.

Images