Robot cost map construction method based on information entropy and semantic weight

Abstract

This invention discloses a robot cost map construction method based on information entropy and semantic weights, belonging to the field of robot environment perception and map construction technology. This invention introduces semantic weights w... class This technology enables differentiated handling of "walls" (requiring high passability) and "pedestrians" (requiring high safety). The robot can be relatively "aggressive" when squeezing through door gaps and relatively "conservative" when avoiding pedestrians, without compromising global parameters. It transforms semantic information extracted from deep learning into "potential field geometry information" understandable by the navigation layer, achieving deep integration of perception and decision-making, and improving the robot's adaptability in unstructured environments. The spatiotemporal uncertainty diffusion technology based on predicted trajectories allows the robot to naturally avoid obstacles based on their movement trends, filling the gap of traditional expansion layers lacking a time dimension. In densely populated scenarios, it significantly reduces the number of sudden stops and path replanning, improving traffic efficiency.

Description

Technical Field

[0001] This invention relates to the field of robot environmental perception and mapping technology, specifically to a robot cost map construction method based on information entropy and semantic weights. Background Technology

[0002] In robot navigation, the cost map serves as a bridge connecting perception and planning. It represents the accessibility of the environment through a grid. The ROS Navigation Stack employs a layered architecture, primarily consisting of a Static Layer, an Obstacle Layer, and an Inflation Layer.

[0003] The standard expansion layer diffuses the lethal cost (254) of an obstacle outward through an exponential decay function, forming a safe potential field. Its calculation formula is as follows:

[0004]

[0005] Where d is the Euclidean distance from the current grid point to the nearest obstacle. λ is the cost scaling factor, which determines the rate at which the cost decreases. inscribed The inner radius of the robot is a pre-defined global static parameter.

[0006] Existing expansion layers have the following limitations:

[0007] 1. Lack of targeted risk assessment:

[0008] The system uses the exact same expansion parameters for a stationary wall, a table, and a moving pedestrian and vehicle. To ensure safety when avoiding pedestrians, λ is usually set very small (slow decay, large buffer), but this can cause the robot to be unable to pass through narrow doorways that would otherwise be passable (excessive wall expansion).

[0009] 2. Difficulty in effectively responding to the movement trends of dynamic obstacles:

[0010] The standard inflation layer is a static snapshot. When a pedestrian approaches, the map only generates a circular inflation at their current coordinates, ignoring their velocity vector. For objects with high uncertainty in their motion (high entropy), a larger safety margin should be reserved, but static maps cannot spatially represent this risk distribution that changes over time, making it difficult to achieve intuitive "avoidance in advance" or "avoidance from behind".

[0011] 3. Loss of semantic information:

[0012] Traditional grid maps simply categorize all obstacles as "occupied" or "free," failing to reflect the semantic information of the obstacles.

[0013] To address the technical issues of existing cost maps being unable to distinguish obstacle properties and adjust safety distances based on real-time risks, a robot cost map construction method based on information entropy and semantic weights is proposed. Summary of the Invention

[0014] The technical problem to be solved by this invention is: how to solve the technical problem that existing cost maps cannot distinguish the nature of obstacles and cannot adjust the safety distance according to real-time risks. This invention provides a robot cost map construction method based on information entropy and semantic weights. This method is a dynamic map generation method that can adjust the local area expansion characteristics in real time according to the semantic attributes (human, glass, wall) and motion state (stationary, stable movement, chaotic movement) of obstacles.

[0015] The present invention solves the above-mentioned technical problems through the following technical solution, and the present invention includes the following steps:

[0016] S1: Parameter Acquisition

[0017] Obtain the semantic ID, location information, and motion entropy value H of the obstacle. motion ;

[0018] S2: Coupling Model Construction

[0019] Construct a semantic-entropy-expansion parameter coupled model and calculate the dynamic decay factor λ. dyn and dynamic expansion radius R dyn ;

[0020] S3: Local expansion injection

[0021] Based on dynamic decay factor λ dyn and dynamic expansion radius R dyn Local expansion injection is performed on the obstacle area;

[0022] S4: Gaussian Dispersion Cost Stacking

[0023] Based on the EKF-CTRV model, Gaussian diffusion cost superposition is carried out on the spatiotemporal uncertainty diffusion of obstacle prediction trajectory;

[0024] S5: Maximum Fusion

[0025] A maximum value fusion strategy is used to fuse the costs of multiple obstacle expansion fields to generate a robot cost map.

[0026] Furthermore, in step S1, the motion entropy value H motionThe covariance matrix is ​​calculated from the EKF covariance matrix of the dynamic obstacle trajectory prediction system and normalized to the [0,1] interval.

[0027] Furthermore, in step S2, the dynamic attenuation factor λ dyn The calculation formula is as follows:

[0028] ;

[0029] Where k represents the k-th dynamic obstacle, λ base w represents the baseline attenuation factor set for a static environment. class β represents the semantic weight coefficient, and β represents the adjustment gain.

[0030] Furthermore, in step S2, the dynamic expansion radius R dyn The calculation formula is as follows:

[0031] ;

[0032] in, R min R is the minimum expansion radius. max This represents the maximum expansion radius.

[0033] Furthermore, in step S3, the specific process of local expansion injection is as follows: taking the center of the obstacle as the origin, in R... dyn Within the range, the Euclidean distance d from each grid cell (x, y) to the obstacle center is calculated using a breadth-first search or iterative computation, and the cost is calculated using a dynamic exponential formula: .

[0034] Furthermore, in step S4, the formula for superimposing Gaussian diffusion costs is as follows:

[0035] ;

[0036] in, This indicates that the obstacle is predicted to appear at this coordinate position at time t. σ t ² represents the variance that increases over time, N=T pred / Δt,T pred To predict the total duration, Δt is the time step.

[0037] Furthermore, in step S5, the final cost formula calculated according to the maximum value fusion strategy is as follows:

[0038] .

[0039] Furthermore, in step S5, the physical coordinates within the calculation range are traversed, centered on the predicted obstacle. Calculate the final cost Afterwards, the final cost will be... Fill in the corresponding map grid to obtain the cost map.

[0040] This invention also provides a robot cost map construction system based on information entropy and semantic weights, applied to the above-mentioned method, including:

[0041] The parameter acquisition module is used to obtain the semantic ID, location information, and motion entropy value H of the obstacle. motion ;

[0042] The coupled model building module is used to construct a semantic-entropy-expansion parameter coupled model and calculate the dynamic decay factor λ. dyn and dynamic expansion radius R dyn ;

[0043] Local expansion injection module, used for dynamic decay factor λ dyn and dynamic expansion radius R dyn Local expansion injection is performed on the obstacle area;

[0044] The Gaussian diffusion cost superposition module performs Gaussian diffusion cost superposition on the spatiotemporal uncertainty diffusion of the obstacle prediction trajectory based on the EKF-CTRV model.

[0045] The maximum value fusion module is used to fuse the costs of multiple obstacle expansion fields using a maximum value fusion strategy to generate a dynamic cost map for the robot.

[0046] The present invention has the following advantages over the prior art:

[0047] 1. Balancing accessibility and security: By introducing semantic weights w class This allows for differentiated handling of "walls" (requiring high passability) and "pedestrians" (requiring high safety). The robot can be relatively "aggressive" when squeezing through door gaps and relatively "conservative" when avoiding pedestrians, without compromising on global parameters.

[0048] 2. Improving map semantic understanding capabilities: Transforming the semantic information extracted by deep learning into "potential field geometric information" that the navigation layer can understand, realizing the deep integration of perception and decision-making, and improving the robot's adaptability in unstructured environments.

[0049] 3. Improved efficiency and safety of path planning in dynamic environments: The spatiotemporal uncertainty diffusion technology based on predicted trajectories enables robots to naturally avoid obstacles according to their movement trends, filling the gap of traditional expansion layers lacking a time dimension. In densely populated scenarios, it significantly reduces the number of sudden stops and path replanning, improving traffic efficiency.

[0050] 4. Optimizations have been made to save computing power to a certain extent: by dynamically adjusting R... dyn This avoids excessive expansion calculations on irrelevant regions of the entire map, allowing the algorithm to maintain high-frequency operation even on embedded industrial control computers with limited computing power. Attached Figure Description

[0051] Figure 1 These are comparison diagrams showing the effect of passing through a static narrow passage in an embodiment of the present invention;

[0052] Figure 2 These are comparison images of dynamic high-risk obstacle avoidance scenarios in embodiments of the present invention;

[0053] Figure 3 This is a comparison diagram of motion trend prediction scenarios in embodiments of the present invention;

[0054] Figure 4 This is a system architecture and data flow diagram in an embodiment of the present invention;

[0055] Figure 5 This is a flowchart illustrating the robot cost map construction method based on information entropy and semantic weights of the present invention. Detailed Implementation

[0056] The embodiments of the present invention are described in detail below. These embodiments are implemented based on the technical solution of the present invention, and provide detailed implementation methods and specific operation processes. However, the scope of protection of the present invention is not limited to the following embodiments.

[0057] This invention proposes a dynamic entropy value inflation layer to replace or enhance the standard inflation layer. Its core lies in constructing a coupling model of "semantics-entropy-inflation parameter".

[0058] 1. A coupling model of semantics and entropy

[0059] The system receives a list of obstacles from the upstream module (dynamic obstacle trajectory prediction system), including the semantic ID, location, and motion entropy value H of each obstacle. motion (Calculated based on the EKF covariance matrix).

[0060] It should be noted that the dynamic obstacle trajectory prediction system of the upstream module is based on the information entropy and the Cooperative Turn Rate and Velocity (CTRV) model. The EKF-CTRV model is a model that uses extended Kalman filtering (EKF) to handle nonlinear motion.

[0061] The following section provides a further explanation of the processing procedures involved in the dynamic obstacle trajectory prediction system:

[0062] Define the system state vector :

[0063]

[0064] Among them, (p x ,p y ) represents the position coordinates, v represents the linear velocity, θ represents the heading angle, and ω represents the yaw rate.

[0065] System state prediction equation It describes the evolution of the state over time, where This is process noise. Due to yaw rate The existence of this factor leads to highly nonlinear position updates. To implement this on a computer, the aforementioned differential equations (system state prediction equations) need to be discretized. Let the sampling time interval be... Integrate over the state variables.

[0066] When ω≠0: State transition function The derivation is as follows:

[0067] Location Update:

[0068]

[0069] Integral result:

[0070]

[0071] Similarly, location Update:

[0072]

[0073] Updates to other state variables:

[0074]

[0075] Based on the above derivation, the complete nonlinear discrete state equation is:

[0076]

[0077] When ω=0 (linear motion), using L'Hôpital's rule to take the limit, we get:

[0078]

[0079] in, This is process noise.

[0080] Derivation of the Jacobian matrix:

[0081] To apply the Extended Kalman Filter (EKF), the aforementioned nonlinear function (state transition function) needs to be expanded into a first-order Taylor series, i.e., the Jacobian matrix needs to be calculated. .

[0082] matrix Let be a 5×5 matrix, let .

[0083] for In this case, the matrix elements are derived as follows:

[0084] Position Partial derivative:

[0085]

[0086] speed Partial derivative:

[0087]

[0088]

[0089] For heading angle Partial derivative:

[0090]

[0091]

[0092] yaw rate Partial derivative:

[0093]

[0094]

[0095] The final Jacobian matrix is ​​as follows:

[0096]

[0097] Among them, the partial derivative with respect to the yaw rate ω is the main factor in dealing with the uncertainty of turning.

[0098] By calculating the Jacobian matrix in real time, EKF can update the state covariance matrix in real time during the prediction phase. This allows us to obtain information about the uncertainty of the motion state.

[0099] Meanwhile, the system introduces Shannon information entropy as a quantitative indicator to measure the unpredictability of obstacle movement.

[0100] For multivariate Gaussian distribution Its differential entropy H(x) is defined as:

[0101]

[0102] In path planning, uncertainties in linear velocity, yaw rate, and heading angle introduce positional uncertainty. Therefore, this invention does not calculate the entropy of the entire state, but instead extracts the sub-covariance matrix related to kinematics.

[0103] set up The 3×3 submatrix corresponding to (v,θ,ω) is Σ motion Let D be 3. Then the motion information entropy H at time k is... motion The calculation formula is:

[0104]

[0105] Where η is the normalization coefficient. When the obstacle's motion is stable, EKF converges, |Σ motion | Decrease, H motion Reduced; when obstacles suddenly change direction or accelerate / decelerate, the prediction error increases, the covariance expands, and H... motion Significantly increased.

[0106] Based on motion entropy H motion A negative feedback adjustment mechanism was constructed to adjust the calculated motion entropy value H. motion The time length T mapped to the predicted trajectory pred To ensure the smoothness of the change, an inverse sigmoid function or a similar decay function is used.

[0107] The mapping formula is as follows:

[0108]

[0109] Among them, T max For the maximum prediction time domain (e.g., 3.0 seconds), it is suitable for low-entropy, high-determinism objectives; T min With a minimum prediction time domain (e.g., 0.2 seconds), it is suitable for high-entropy, chaotic moving targets; Entropy threshold, corresponding to the inflection point of the system's tolerance to uncertainty, is obtained from sensor static noise level calibration or specific experiments. α is the sensitivity coefficient, controlling the decay rate of the entropy value over time. Different sensitivity coefficients can be set for different objects being identified; for example, the sensitivity should be higher for children, while the sensitivity can be appropriately lowered for AGVs moving at a constant speed.

[0110] when When the denominator approaches 1, T pred →T max The system generates long-distance trajectories, allowing the planner to perform long-term planning.

[0111] when When the denominator approaches infinity, T pred →T min The system truncates long-term trajectories, retaining only the current instantaneous state, to prevent the planner from being misled by erroneous long-term predictions.

[0112] Ultimately, the system utilizes the current state. and the calculated dynamic time domain T pred A series of discrete prediction points are generated through iterative generation of the CTRV equation. , where N=T pred / Δt. These point sets will be encapsulated into standard ROS messages and published to the local path planner.

[0113] This invention defines two dynamic adjustment functions: dynamic decay factor λ dyn and dynamic expansion radius R dyn .

[0114] (1) Dynamic decay factor λ dyn :

[0115] The attenuation factor λ determines the steepness of the potential field. The smaller λ is, the wider the distribution of high-cost regions.

[0116] In traditional formulas, the cost decreases exponentially with distance. This invention introduces a dynamic decay factor, as shown in the following formula:

[0117]

[0118] Where k represents the k-th dynamic obstacle, λ base This represents the baseline attenuation factor set for static environments (it can be set to a larger value to ensure the passability of static channels), w class This represents the semantic weight coefficient. It reflects the risk assessment for different categories of objects, H. motion β represents the normalized motion entropy value (range 0~1), derived from EKF estimation, and β represents the adjustment gain.

[0119] In this embodiment, the semantic weight coefficient w class The specific semantic ID corresponds to the semantic ID, which depends on the semantic ID identified by the image recognition algorithm (such as the YOLO algorithm). class It has been pre-set, for example, setting w person =2.0 (high sensitivity), w table =0.1 (low sensitivity), for example, the w of a cat. cat w greater than person person .

[0120] It should be noted that image recognition algorithms serve as the front-end input to the dynamic obstacle trajectory prediction system. Image recognition algorithms (such as the YOLO algorithm) extract the semantic ID of obstacles and their pixel coordinates in the RGB image in real time. The obstacle's semantic ID category corresponds to a pre-defined semantic weight coefficient (as described above), and the RGB pixel coordinates, combined with RGB-D depth data, are converted into three-dimensional spatial coordinates (the obstacle's position) in a map coordinate system. The obstacle's semantic ID and position are then input into the dynamic obstacle trajectory prediction system for further processing.

[0121] Working principle explanation:

[0122] For static obstacles: w class Set to a very low value or 0, H motion ≈0. At this time, λ dyn ≈λ base This means that the cost value decreases rapidly with distance, and the expansion area is relatively concentrated. At this time, the robot is allowed to still believe that the path is passable when it is close to the door frame, thus solving the problem of difficulty in passing through narrow passages.

[0123] For high-risk dynamic obstacles, such as pedestrians and children, w class Set to a larger value, and because of H motion The value is relatively high, at which point λ dyn The value decays very slowly, significantly reducing the risk of loss and creating a wider warning field that forces the robot to begin circling away from a distance to maintain a safe distance.

[0124] In this embodiment, the static narrow channel has a similar effect to, for example Figure 1 As shown.

[0125] (2) Dynamic expansion radius R dyn :

[0126] This invention no longer performs large-radius expansion on the entire map, but instead allocates computational resources on demand based on uncertainty, dynamically expanding the radius R. dyn The calculation formula is as follows:

[0127]

[0128] in, R min R is the minimum expansion radius. max This represents the maximum expansion radius.

[0129] The Sigmoid function is used to map entropy values ​​to the expansion radius interval [R]. min ,R max This ensures that the physical impact range of high-risk objects is explicitly expanded on the map.

[0130] Working principle explanation:

[0131] For static obstacles: only calculate R. min Within the range of costs, save computing power.

[0132] For high-risk dynamic obstacles: Activate R max The range is calculated by using the Sigmoid function to map the entropy value to the expansion radius interval [R]. min , R max This ensures that the physical impact range of high-risk objects is explicitly expanded on the map. It also allows for the selective concentration of computing power while ensuring security. The final result is as follows: Figure 2 As shown.

[0133] 2. Spatiotemporal uncertainty diffusion based on predicted trajectories

[0134] In addition to circular expansion centered on the current location, this invention also introduces a spatiotemporal uncertainty diffusion technique based on predicted trajectories. This aims to address the shortcomings of the standard expansion layer's "static snapshot" by utilizing trajectory prediction results for Gaussian diffusion. The specific method is as follows:

[0135] Prediction point set obtained based on dynamic obstacle trajectory prediction system A series of Gaussian distributions are superimposed on the map along the predicted path, representing the cost. As the prediction time t increases, the variance σ of the Gaussian distribution... t ² Gradually increases (simulating the diffusion of positional uncertainty), for which we designed the following formula:

[0136]

[0137] in, This indicates that the obstacle is predicted to appear at this coordinate position at time t. σ t ² represents the variance that increases over time, controlling the size of the danger circle. N=T pred / Δt,T pred To predict the total duration, Δt is the time step.

[0138] This process generates a comet-tail-shaped asymmetric high-risk region on the map, extending along the direction of movement. Mathematically, it constructs a probabilistic potential field, telling the local path planner, "Although this area is currently empty, there's a high probability someone will enter later; don't rush in front of this object," thus guiding the robot to detour around obstacles, which is more in line with human behavior. The final result is as follows... Figure 3 As shown.

[0139] 3. Software implementation process of the dynamic layer

[0140] Based on the above model, this invention is implemented as a custom CostmapPlugin in ROS Navigation. Its processing flow is as follows:

[0141] (1) Data subscription: Real-time reception of data from upstream modules: {ID, Position, H} motion}

[0142] (2) Area Reset: When updating each frame, first clear the value written on the map by the dynamic layer of the previous frame, and restore the static map data of the underlying layer.

[0143] (3) Parameter calculation: Traverse all obstacles and calculate the personalized dynamic attenuation factor λ for each object according to the above formula. dyn and dynamic expansion radius R dyn .

[0144] (4) Local expansion injection:

[0145] Taking the center (cx, cy) of each obstacle as the origin, in R dyn Perform breadth-first search (BFS) or iterative computation within the range.

[0146] Calculate the Euclidean distance d from each grid cell (x,y) within this range to the center of the obstacle.

[0147] Calculate the cost using the dynamic exponential formula: .

[0148] (5) The final cost of the maximum value fusion strategy: In order to handle the superposition of multiple expansion fields (e.g., a person standing by a wall), the maximum value principle is adopted:

[0149]

[0150] This ensures that the introduction of dynamic layers will only increase security (at a higher cost) and will not mistakenly clear information about static obstacles.

[0151] (6) Using the predicted obstacle as the center, traverse the corresponding physical coordinates within the calculation range. Calculate Then, it is filled into the corresponding map grid, and finally a custom Costmap Plugin layer is generated, which is the dynamic cost map.

[0152] System process as follows Figure 4 As shown.

[0153] In summary, the specific decision-making of the system in different scenarios is summarized in Table 1 below.

[0154] Table 1 Specific Decisions in Different Scenarios

[0155]

[0156] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of the present invention.

Claims

1. A method for constructing robot cost maps based on information entropy and semantic weights, characterized in that, Includes the following steps: S1: Parameter Acquisition Obtain the semantic ID, location information, and motion entropy value H of the obstacle. motion ; S2: Coupling Model Construction Construct a semantic-entropy-expansion parameter coupled model and calculate the dynamic decay factor λ. dyn and dynamic expansion radius R dyn ; S3: Local expansion injection Based on dynamic decay factor λ dyn and dynamic expansion radius R dyn Local expansion injection is performed on the obstacle area; S4: Gaussian Dispersion Cost Stacking Based on the EKF-CTRV model, Gaussian diffusion cost superposition is carried out on the spatiotemporal uncertainty diffusion of obstacle prediction trajectory; S5: Maximum Fusion A maximum value fusion strategy is used to fuse the costs of multiple obstacle expansion fields to generate a robot cost map.

2. The robot cost map construction method based on information entropy and semantic weights according to claim 1, characterized in that, In step S1, the motion entropy value H motion The covariance matrix is ​​calculated from the EKF covariance matrix of the dynamic obstacle trajectory prediction system and normalized to the [0,1] interval.

3. The robot cost map construction method based on information entropy and semantic weights according to claim 1, characterized in that, In step S2, the dynamic attenuation factor λ dyn The calculation formula is as follows: ； Where k represents the k-th dynamic obstacle, λ base w represents the baseline attenuation factor set for a static environment. class β represents the semantic weight coefficient, and β represents the adjustment gain.

4. The robot cost map construction method based on information entropy and semantic weights according to claim 3, characterized in that, In step S2, the dynamic expansion radius R dyn The calculation formula is as follows: ； in, R min R is the minimum expansion radius. max This represents the maximum expansion radius.

5. The robot cost map construction method based on information entropy and semantic weights according to claim 1, characterized in that, In step S3, the specific process of local expansion injection is as follows: taking the center of the obstacle as the origin, in R... dyn Within the range, the Euclidean distance d from each grid cell (x, y) to the obstacle center is calculated using a breadth-first search or iterative computation, and the cost is calculated using a dynamic exponential formula: .

6. The robot cost map construction method based on information entropy and semantic weights according to claim 5, characterized in that, In step S4, the formula for superimposing Gaussian dispersion costs is as follows: ； in, This indicates that the obstacle is predicted to appear at this coordinate position at time t. σ t ² represents the variance that increases over time, N=T pred / Δt,T pred To predict the total duration, Δt is the time step.

7. The robot cost map construction method based on information entropy and semantic weights according to claim 6, characterized in that, In step S5, the final cost formula calculated according to the maximum value fusion strategy is as follows: 。 8. The robot cost map construction method based on information entropy and semantic weights according to claim 7, characterized in that, In step S5, the physical coordinates within the calculation range are traversed, centered on the predicted obstacle. Calculate the final cost Afterwards, the final cost will be... Fill in the corresponding map grid to obtain the cost map.

Images