Method for constructing occupied grid map based on distance weighting

Abstract

The invention discloses an occupation grid map construction method based on distance weighting, and belongs to the field of intelligent vehicle digital maps and high-precision positioning. The method comprises the following steps: acquiring pose data of an intelligent vehicle and sensing data of a surrounding environment; constructing a distance weight factor; generating an occupation grid map according to the obtained pose data of the intelligent vehicle and the perception data of the surrounding environment, dividing the occupation grid map based on the Bayesian theorem and a distance weight factor, and endowing each grid with state information; and dynamically updating the occupied grid map by using a double-end linked list structure. According to the method, the distance weight factor is constructed, and the grid map is divided through the Bayesian theorem and the distance weight factor, so that the obstacle state can be accurately expressed in a dynamic non-structural environment, and the problem of track noise is avoided.

Description

Technical Field

[0001] The present invention belongs to the field of intelligent vehicle digital maps and high-precision positioning, and in particular relates to a distance-weighted occupancy grid map construction method. Background Art

[0002] Intelligent vehicles primarily consist of three modules: perception, planning, and control. These modules work together to support the efficient operation of intelligent transportation systems. The perception module uses sensors to collect real-time information about the surrounding environment. The planning module plans paths and trajectories based on this perception data. The control module ensures that the vehicle safely follows the planned trajectory. Occupancy grid maps, as an effective representation of the environment, are widely used in trajectory planning algorithms.

[0003] However, in dynamic environments, grid maps pose greater challenges than in structured environments due to the large variety and chaotic distribution of obstacles and their irregular movement patterns. Traditional methods for constructing occupancy grid maps struggle to accurately represent dynamic obstacles in the environment and often generate trajectory noise, resulting in unreliable planned trajectories and potentially threatening the safe operation of intelligent vehicles. Therefore, how to quickly and accurately construct occupancy grid maps in complex, dynamic, unstructured environments is a key challenge that urgently needs to be overcome in the field of intelligent vehicle technology. Summary of the Invention

[0004] In order to solve the above technical problems, the present invention provides a distance-weighted dynamic environment occupancy grid map construction method.

[0005] To implement the above technique, the steps are as follows:

[0006] S1, obtain the position data of the smart vehicle and the perception data of the surrounding environment through smart sensors;

[0007] S2, constructing a distance weight factor based on an algorithm that considers distance weights and the distance from each grid to the nearest obstacle;

[0008] The distance weight factor is expressed as follows:

[0009]

[0010] Where, d obs Indicates the distance from the current grid to the nearest obstacle; α is the first adjustment parameter, which is used to control the steepness of the change in the weight factor function, that is, it determines the sensitivity of the grid state to distance changes. The larger the value, the faster the response to distance; β is the second adjustment parameter, which is used to set the critical position where the weight changes significantly and adjust the area where the system focuses on.

[0011] S3. Generate an occupancy grid map based on the obtained intelligent vehicle posture data and the perception data of the surrounding environment, and divide the occupancy grid map based on the Bayesian theorem and the distance weight factor, and assign each grid state information, where the state information includes: idle, occupied, or unknown;

[0012] Based on Bayesian theorem and distance weight factor, the grid map is divided into the following: Bayesian theorem processes sensor observation data, updates the grid occupancy probability according to each sensor observation, and adjusts the grid state through recursive updating. The specific update formula is:

[0013]

[0014] Where, and Represents the grid m at time t and t-1 respectively i The logarithmic occupancy probability of Represents grid m at time t i Logarithmic update state; where p represents the probability; z t represents the observed value; z 1:t represents the observation value at time t; z 1:t-1 represents the observation value at time t-1; Represents grid m i Idle state; p(m i |z 1:t ) represents the observation data z based on 1 to t times at time t 1:t Under the condition of grid m i The probability of being occupied, Represents the observation data z based on 1 to t times at time t 1:t Under the condition of grid m i The probability of being idle, p(m i |z 1:t-1 ) represents the observation data z based on 1 to t-1 at time t-1 1:t-1 Under the condition of grid m i The probability of being occupied, Represents the observation data z based on 1 to t-1 at time t-1 1:t-1 Under the condition of grid m i Probability of being idle;

[0015] S4. Based on the obtained state information of each grid, dynamically update the occupied grid map using a double-ended linked list structure to complete the construction of the occupied grid map;

[0016] The double-ended linked list structure includes a head node pointer and a tail node pointer for identifying the coordinates of the nearest obstacle, as well as an insertion and removal strategy triggered by grid state changes. This structure enables efficient storage and management of grid obstacle information, supporting rapid insertion, deletion, and update operations on grid nodes when the state changes, effectively reducing computational complexity and improving the system's real-time processing performance.

[0017] The working method of the double-ended linked list structure is as follows: during the grid map update process, if the status of a grid changes from "unknown" or "unoccupied" to "occupied", the system first removes the grid from its original double-ended linked list and updates its nearest obstacle information; at this time, the coordinates of the nearest obstacle of the grid are set to its own position, indicating that the grid has become a new obstacle; then, the system inserts the grid into a new linked list identified by its own coordinates and adds it to the list to be updated in order to trigger subsequent processing of the area associated with it; if the grid status changes from "occupied" to "unoccupied", the system traverses all related grids in the linked list where the current grid is located. , remove them from the linked list and set their nearest obstacle coordinates to the infinity point to clear their original obstacle-related information; then, the system further traverses the neighboring grids of these grids to determine whether their nearest obstacle exists, and compares the distance from the obstacle to the current grid with the shortest distance currently recorded; if the former is closer, the nearest obstacle coordinates of the current grid are updated to the nearest obstacle coordinates of the neighboring grid; based on the update result, the system then inserts the grid into the corresponding linked list: if the updated coordinates are the infinity point, then it is inserted into the infinity point linked list; if they are valid coordinates, then it is inserted into the corresponding obstacle linked list and added to the list to be updated;

[0018] The entire update process recalculates the nearest obstacle information of its neighboring grids by continuously traversing and processing the grids in the list to be updated. If it is found during the processing that the distance from a neighboring grid to the new obstacle is shorter than its currently recorded distance, the obstacle coordinates of the neighboring grid will be updated and added to the queue to be updated again. This process continues to iterate until the obstacle information of all relevant grids is updated. This achieves fast and adaptive updating of the nearest obstacle coordinates in a dynamic environment, ensuring the stability and accuracy of the occupancy grid map in complex environments.

[0019] Beneficial effects of the present invention:

[0020] By constructing a distance weight factor and dividing the grid map using the Bayesian theorem and the distance weight factor, the present invention can accurately express the obstacle status in a dynamic unstructured environment and avoid the trajectory noise problem.

[0021] The present invention is not only suitable for path planning and obstacle avoidance tasks of autonomous driving vehicles in typical unstructured road scenarios such as urban traffic and campus roads, but can also be widely used in mobile robots in application fields that require real-time response to dynamic environments, such as warehousing and logistics, medical services, and security inspections. It provides reliable environmental perception support for autonomous mobile systems in complex environments, and has broad application prospects and engineering value. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 is a flow chart of the present invention;

[0023] Figure 2 Schematic diagram of the grid structure of the present invention;

[0024] Figure 3 This is a flow chart of raster processing;

[0025] Figure 4 Schematic diagram of the simulation scene;

[0026] Figure 5 The grid map construction effect diagram of the simulation scene of the algorithm of the present invention and the costmap_2d algorithm when the grid resolution is 0.2m×0.2m;

[0027] Figure 6 Schematic diagram of a highly dynamic simulation scenario;

[0028] Figure 7 The grid map construction effect diagram of the algorithm of the present invention and the costmap_2d algorithm at a grid resolution of 0.1m×0.1m for a highly dynamic simulation scene; wherein, part (a) shows the result at the initial moment; part (b) shows the result when tracking delay occurs; part (c) shows the result when tail noise occurs; and part (d) shows the result when tail noise persists. DETAILED DESCRIPTION

[0029] The present invention is further described in detail below with reference to specific embodiments.

[0030] like Figure 1 As shown, a distance-weighted dynamic environment occupancy grid map construction method includes the following steps:

[0031] S1, obtain the position data of the smart vehicle and the perception data of the surrounding environment through smart sensors;

[0032] Inertial navigation is based on Newton's laws of motion. It uses an inertial measurement unit to measure the linear and angular motion of a vehicle in inertial space, and then calculates the position, velocity, and attitude of the vehicle based on the navigation differential equation.

[0033] The acquisition methods include: obtaining vehicle posture data through the inertial measurement unit (IMU); obtaining surrounding environment perception data through multi-line laser radar;

[0034] S2, constructing a distance weight factor based on an algorithm that considers distance weights and the distance from each grid to the nearest obstacle;

[0035] Use an algorithm that considers distance weights to calculate the distance from each grid to the nearest obstacle, and use this distance as a weighting factor to adjust the update weight of the grid;

[0036] Based on the calculated distance weight, the grid status is updated weightedly, with grids close to obstacles being updated first. This ensures that obstacle information can be quickly reflected and the map status adjusted in a dynamic environment.

[0037] The distance weight factor is expressed as follows:

[0038]

[0039] Where, d obs Represents the distance from the current grid to the nearest obstacle; α represents the first adjustment parameter, which is used to control the steepness of the change in the weight factor function, that is, it determines the sensitivity of the grid state to distance changes. The larger the value, the faster the response to distance. In this embodiment, α is -10; β represents the second adjustment parameter, which is used to set the critical position for significant weight changes and adjust the area that the system focuses on. In this embodiment, β is 0.4;

[0040] The distance weighting factor makes grids closer than 0.4 meters to obstacles more sensitive to environmental changes during map updates, enabling more timely responses. Therefore, the system assigns higher weights to these grids when updating their status. Grids farther away from obstacles, which experience less environmental change, have their update priority lowered, thereby improving the overall system's processing efficiency in dynamic scenarios.

[0041] The distance weight factor can be used to reduce the wake noise problem caused by dynamic obstacles, which helps to improve the accuracy of the occupancy grid map in dynamic environments.

[0042] S3, generate an occupancy grid map based on the obtained intelligent vehicle posture data and the perception data of the surrounding environment, such as Figure 2 As shown;

[0043] The occupancy grid map is divided based on Bayesian theorem and distance weight factor, and each grid is given status information, which includes: idle, occupied or unknown;

[0044] Based on Bayesian theorem and distance weight factor, the grid map is divided into the following: Bayesian theorem processes sensor observation data, updates the grid occupancy probability according to each sensor observation, and adjusts the grid state through recursive updating. The specific update formula is:

[0045]

[0046] Where, and Represents the grid m at time t and t-1 respectively i The logarithmic occupancy probability of Represents grid m at time t i Logarithmic update state; where p represents the probability; z t represents the observed value; z 1:t represents the observation value at time t; z 1:t-1 represents the observation value at time t-1; Represents grid m i Idle state; p(m i |z 1:t ) represents the observation data z based on 1 to t times at time t 1:t Under the condition of grid m i The probability of being occupied, Represents the observation data z based on 1 to t times at time t 1:t Under the condition of grid m i The probability of being idle, p(m i |z 1:t-1 ) represents the observation data z based on 1 to t-1 at time t-1 1:t-1 Under the condition of grid m i The probability of being occupied, Represents the observation data z based on 1 to t-1 at time t-1 1:t-1 Under the condition of grid m i Probability of being idle;

[0047] In this embodiment, at the initial moment, the lower grid m i The logarithmic occupancy probability Set to 0, the grid m at any subsequent time i The logarithmic occupancy probability is recursively updated by the formula; when the grid m i When it is determined to be occupied at time t, the corresponding logarithmic update state Set to 0.9; if it is judged to be idle, set to -0.7; grid m i The state of the vehicle is determined by the data collected by the multi-line lidar sensor.

[0048] S4, such as Figure 3As shown, based on the obtained state information of each grid, the occupied grid map is dynamically updated using a double-ended linked list structure to complete the construction of the occupied grid map;

[0049] A double-ended linked list structure is used to store the coordinates of the nearest obstacle for each grid. Whenever the obstacle status changes, the obstacle information associated with that grid is quickly updated. This optimizes the obstacle distance calculation between grids and ensures accurate and real-time obstacle information in dynamic environments.

[0050] The double-ended linked list structure includes a head node pointer and a tail node pointer for identifying the coordinates of the nearest obstacle, as well as an insertion and removal strategy triggered by grid state changes. This structure enables efficient storage and management of grid obstacle information, supporting rapid insertion, deletion, and update operations on grid nodes when the state changes, effectively reducing computational complexity and improving the system's real-time processing performance.

[0051] The working method of the double-ended linked list structure is as follows: during the grid map update process, if the status of a grid changes from "unknown" or "unoccupied" to "occupied", the system first removes the grid from its original double-ended linked list and updates its nearest obstacle information; at this time, the coordinates of the nearest obstacle of the grid are set to its own position, indicating that the grid has become a new obstacle; then, the system inserts the grid into a new linked list identified by its own coordinates and adds it to the list to be updated in order to trigger subsequent processing of the area associated with it; if the grid status changes from "occupied" to "unoccupied", the system traverses all related grids in the linked list where the current grid is located. , remove them from the linked list and set their nearest obstacle coordinates to the infinity point to clear their original obstacle-related information; then, the system further traverses the neighboring grids of these grids to determine whether their nearest obstacle exists, and compares the distance from the obstacle to the current grid with the shortest distance currently recorded; if the former is closer, the nearest obstacle coordinates of the current grid are updated to the nearest obstacle coordinates of the neighboring grid; based on the update result, the system then inserts the grid into the corresponding linked list: if the updated coordinates are the infinity point, then it is inserted into the infinity point linked list; if they are valid coordinates, then it is inserted into the corresponding obstacle linked list and added to the list to be updated;

[0052] The entire update process recalculates the nearest obstacle information of its neighboring grids by continuously traversing and processing the grids in the list to be updated. If it is found during the processing that the distance from a neighboring grid to the new obstacle is shorter than its currently recorded distance, the obstacle coordinates of the neighboring grid will be updated and added to the queue to be updated again. This process continues to iterate until the obstacle information of all relevant grids is updated. This achieves fast and adaptive updating of the nearest obstacle coordinates in a dynamic environment, ensuring the stability and accuracy of the occupancy grid map in complex environments.

[0053] In order to verify that the present invention can achieve more accurate environmental perception in a dynamic unstructured environment and effectively avoid the trajectory noise problem, the method of the present invention is compared with the costmap_2d algorithm.

[0054] like Figure 4 As shown in the figure, a Gazebo simulation environment is established. The pedestrian in the box in the environment serves as a dynamic obstacle, moving back and forth along a straight line at a speed of 1.5m / s, and the other cubes are static obstacles.

[0055] like Figure 5 Figure 2 shows the occupancy grid maps constructed by the two algorithms when t is 16 and the grid resolution is 0.2m×0.2m. The map size is 50×50 grid cells. It can be seen that the costmap_2d algorithm has obvious trajectory noise problems when dealing with dynamic obstacles, while the method of the present invention can accurately express the location information of dynamic obstacles.

[0056] In order to analyze the generation process of wake noise more comprehensively, a comprehensive comparative analysis of the performance of this algorithm and the costmap_2d algorithm is conducted. Figure 6 Further experiments were conducted using a dynamic simulation scenario in

[15] . Six pedestrians were set up in the simulation scene as moving obstacles. These obstacles also moved back and forth along a straight line at a speed of 1.5m / s to simulate a complex dynamic unstructured environment.

[0057] Figure 7 The figure shows the different performance moments of the algorithm in this paper and the costmap_2d algorithm in the process of building an occupied grid map. The constructed map size is 100×100 grid cells and the grid resolution is 0.1m×0.1m. Figure 5 To maintain consistency, the gray area represents the occupancy grid map constructed by our algorithm, and the black area represents the occupancy grid map constructed by the costmap_2d algorithm. In addition, an arrow is used to mark the current direction of movement of each dynamic obstacle. Figure 7 Part (a) is the time when the map is built. t is 0, when the two algorithms start to build the map. The movement of dynamic obstacles has not yet affected the algorithm construction process. Figure 7 At the moment t = 8 shown in part (b), the costmap_2d algorithm has already experienced tracking delay, that is, the constructed occupancy grid map has lagged behind the moving obstacle. However, the proposed algorithm can still accurately track and accurately represent the position of the dynamic obstacle. Figure 7(c) At this moment, t is 15. As the dynamic obstacle continues to move, the tracking delay of the costmap_2d algorithm continues and cannot be effectively corrected. The tracking delay phenomenon is further aggravated. Figure 7 At the moment shown in part (d), t is 25. At this time, the occupancy grid map constructed by the costmap_2d algorithm has obvious tail noise, but the present invention can still accurately express the dynamic obstacles.

[0058] The distance-weighted occupancy grid map construction method proposed in this paper has excellent adaptability and can achieve more accurate environmental grid representation in dynamic environments, providing strong support for subsequent trajectory planning. This method is particularly suitable for grid map representation of autonomous vehicles in unstructured traffic scenarios such as open campuses, and is also suitable for path planning and navigation of mobile robots in mixed human and vehicle traffic areas such as warehouses, hospitals, and airports. Therefore, this invention has broad application prospects and can provide strong support for the stable operation of intelligent mobile systems in real-world complex scenarios.

[0059] The above descriptions are only partial embodiments of the present invention. It should be pointed out that ordinary technicians in this technical field can make several improvements and modifications without departing from the principles of the present invention. These improvements and modifications should also be regarded as within the scope of protection of the present invention.

Claims

1. A distance-weighted dynamic environment occupancy grid map construction method, characterized in that: The following steps are involved: S1, obtain the position data of the smart vehicle and the perception data of the surrounding environment through smart sensors; S2, constructing a distance weight factor based on an algorithm that considers distance weights and the distance from each grid to the nearest obstacle; S3. Generate an occupancy grid map based on the obtained intelligent vehicle posture data and the perception data of the surrounding environment, and divide the occupancy grid map based on the Bayesian theorem and the distance weight factor, and assign each grid state information, where the state information includes: idle, occupied, or unknown; S4. Based on the obtained state information of each grid, the occupied grid map is dynamically updated using a double-ended linked list structure to complete the construction of the occupied grid map.

2. A distance-weighted dynamic environment occupancy grid map construction method according to claim 1, characterized in that: The algorithm based on distance weight and the distance from each grid to the nearest obstacle constructs the expression of the distance weight factor as follows: Where, d obs Represents the distance from the current grid to the nearest obstacle; α represents the first adjustment parameter, which is used to control the steepness of the change of the weight factor function; β represents the second adjustment parameter, which is used to set the critical position of the weight.

3. The method for constructing a dynamic environment occupancy grid map based on distance weighting according to claim 1, characterized in that: The occupancy grid map is generated based on the obtained posture data of the intelligent vehicle and the perception data of the surrounding environment, and the occupancy grid map is divided based on the Bayesian theorem and the distance weight factor, and each grid is assigned state information. The state information includes: idle, occupied, or unknown. The method of dividing the occupancy grid map based on the Bayesian theorem and the distance weight factor is as follows: the Bayesian theorem processes the sensor observation data, updates the occupancy probability of the grid according to each sensor observation, and adjusts the grid state through a recursive update method. The expression is as follows: Where, and Represents the grid m at time t and t-1 respectively i The logarithmic occupancy probability of Represents grid m at time t i Logarithmic update state; where p represents the probability; z t represents the observed value; z 1:t represents the observation value at time t; z 1:t-1 represents the observation value at time t-1; Represents grid m i Idle state; p(m i |z 1:t ) represents the observation data z based on 1 to t times at time t 1:t Under the condition of grid m i The probability of being occupied, Represents the observation data z based on 1 to t times at time t 1:t Under the condition of grid m i The probability of being idle, p(m i |z 1:t-1 ) represents the observation data z based on 1 to t-1 at time t-1 1:t-1 Under the condition of grid m i The probability of being occupied, Represents the observation data z based on 1 to t-1 at time t-1 1:t-1 Under the condition of grid m i Probability of being idle.

4. The distance-weighted dynamic environment occupancy grid map construction method according to claim 1, characterized in that: Based on the obtained state information of each grid, the occupied grid map is dynamically updated using a double-ended linked list structure to complete the construction of the occupied grid map. The double-ended linked list structure includes: a head node pointer and a tail node pointer for identifying the coordinates of the nearest obstacle, an insertion strategy and a clearing strategy triggered by grid state changes.

Images