Self-adaptive expansion radius adjusting method based on cost map
By acquiring the cost map width, vehicle size, and vehicle speed in real time, and calculating the adaptive expansion radius, the safety and passability issues of fixed expansion radius in complex scenarios are solved, enabling efficient navigation of the robot in complex environments.
Patent Information
- Application Number
- CN202511245397.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-09-02
- Publication Date
- 2026-03-03
AI Technical Summary
In existing autonomous mobile robot navigation methods, the fixed expansion radius cannot adapt to scale changes in complex scenarios, and the robot's geometry and real-time speed are not taken into account, resulting in insufficient safety and maneuverability.
By acquiring the cost map width, vehicle size, and vehicle speed in real time, the adaptive expansion radius is calculated. Combined with the dynamic safety margin coefficient, the expansion radius is dynamically adjusted to adapt to changes in environment and speed.
It improves the accessibility and efficiency of path planning, enhances the safety and responsiveness of robots in complex environments, and adapts to the scalability of different platforms.
Smart Images
Figure CN121594871A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of autonomous mobile robots, and in particular to a method for adaptively adjusting the expansion radius based on the overall width of the cost map, the vehicle's geometric dimensions, and the real-time vehicle speed, and its application in mobile robot systems. Background Technology
[0002] In the field of autonomous mobile robot navigation, path planning methods based on two-dimensional cost maps and inflation layers are widely used. This method establishes a safety buffer zone by expanding obstacles outward by a certain range, and is the mainstream technical means to ensure the collision safety of mobile platforms.
[0003] However, existing methods mostly use fixed values or expansion radii based on simple rules, which have significant limitations: First, they cannot adapt to scale changes in complex scenarios, and it is difficult to balance safety and passability in narrow and open environments; Second, they do not consider the actual geometric dimensions of the robot body, resulting in a lack of targeted safety configuration; Third, they fail to integrate dynamic state information such as real-time speed, and cannot adaptively adjust the safety margin according to the movement speed, which poses a collision risk under high-speed conditions.
[0004] Therefore, current technology urgently needs an optimization scheme that can comprehensively perceive the environmental structure, vehicle size and real-time motion status, and adaptively adjust the expansion radius online accordingly, so as to systematically improve the navigation safety, flexibility and operational efficiency of robots in dynamic and complex scenarios. Summary of the Invention
[0005] The problem this invention aims to solve is how to provide an adaptive expansion radius adjustment method and system that can adapt to complex scenarios and balance safety and passability.
[0006] To solve the above-mentioned technical problems, the technical solution adopted by the present invention is: an adaptive expansion radius adjustment method based on a cost map, characterized by the following steps:
[0007] S1, real-time acquisition of the overall width Wc of the cost map;
[0008] S2, obtain the geometric envelope dimensions of the vehicle body, including the vehicle width Wv and the vehicle length Lv;
[0009] S3. Calculate the dynamic safety margin coefficient k(v) based on the real-time vehicle speed v, where k(v) is a monotonically increasing function of vehicle speed v.
[0010] S4, considering the overall width Wc, vehicle dimensions Wv and Lv, and the dynamic safety margin coefficient k(v), α and β are empirical coefficients, Rbase is the baseline expansion radius, and Rmax is the maximum allowable expansion radius of the system. The adaptive expansion radius R is calculated using the following formula:
[0011]
[0012] S5, write the calculated expansion radius R into the expansion layer of the cost map, and trigger the path planning module to replan or locally correct the path.
[0013] The present invention also discloses an adaptive expansion radius adjustment system, characterized in that it includes: a map width extraction module, a vehicle body parameter interface module, a vehicle speed sensing module, an adaptive calculation unit, and a map update interface module.
[0014] In S1, the overall width Wc is calculated to obtain the maximum continuous free space width through grid traversal or sliding window of the cost map.
[0015] In S2, the vehicle body geometric envelope dimensions Wv and Lv are read from the vehicle body model database.
[0016] In S3, the real-time vehicle speed v is obtained through a wheel speed encoder, IMU, or CAN bus; the dynamic safety margin coefficient k(v) satisfies:
[0017] The beneficial effects of this invention are as follows:
[0018] The adaptive expansion radius adjustment method can adjust the safety boundary in real time according to the width of the environmental passage, effectively solving the problems of traditional fixed expansion radius being unable to pass through narrow areas and having excessive planning redundancy in open areas, thus improving the passability and efficiency of path planning.
[0019] Taking into account the actual physical size of the robot, the geometric envelope of the vehicle body is used as the key parameter for radius calculation, which overcomes the problem of poor adaptability of previous methods to robots of different sizes and improves the scalability and practicality of the system to different platforms.
[0020] This invention introduces real-time vehicle speed information and achieves intelligent adjustment of redundancy at high speeds and redundancy reduction at low speeds through a dynamic safety margin coefficient. This solves the hidden danger of insufficient safety in high-speed scenarios by traditional methods and significantly improves the motion safety and response robustness of robots under dynamic and high-speed conditions. Attached Figure Description
[0021] To more clearly illustrate the implementation of the present invention or the existing technical solutions, the accompanying drawings used in the description of the embodiments or the prior art will be briefly introduced below.
[0022] Figure 1 Flowchart for calculating the expansion radius
[0023] The process of obtaining data from the map width calculation unit, vehicle size acquisition unit, and vehicle speed acquisition unit, and then calculating the expansion radius is described.
[0024] The expansion radius calculation unit receives data from the above three units and processes it.
[0025] After the calculation is complete, the update interface unit passes the result to the cost map inflation layer. Detailed Implementation Plan
[0026] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the protection scope of the present invention.
[0027] Many specific details are set forth in the following description in order to provide a full understanding of the invention. However, the invention may also be practiced in other ways different from those described herein, and those skilled in the art can make similar extensions without departing from the spirit of the invention. Therefore, the invention is not limited to the specific embodiments disclosed below.
[0028] like Figure 1 As shown in the figure, this invention discloses an adaptive expansion radius adjustment method based on a cost map, comprising the following steps:
[0029] S101: Real-time extraction of the overall passage width Wc of passable areas in the cost map;
[0030] S102: Obtain the vehicle body envelope dimensions from the robot parameter configuration library or URDF model, including the vehicle body width Wv and the vehicle body length Lv;
[0031] S103: Real-time vehicle speed v is acquired via wheel encoder, IMU or CAN bus;
[0032] S104: Calculate the dynamic safety margin coefficient k(v) based on the real-time vehicle speed v;
[0033] S105: Calculate the adaptive expansion radius R by combining Wc, Wv, Lv, k(v) and preset parameters α, β, R_base, and R_max;
[0034] S106: Write the expansion radius R into the cost map expansion layer and trigger the path planning module to perform trajectory replanning or local correction.
[0035] The dynamic safety margin coefficient k(v) in step S104 is calculated as follows: based on the following nonlinear function:
[0036] In step S105, calculating the adaptive expansion radius R includes the following specific steps:
[0037] Calculate the candidate radius based on the environment width respectively. ;
[0038] Calculate candidate radii based on vehicle body dimensions ;
[0039] Calculate velocity-based candidate radii The minimum value between the above candidate radii and the system's maximum allowable radius R_max is taken as the final expansion radius, i.e.: .
[0040] like Figure 2 As shown: Corresponding to the above method, this embodiment of the invention also discloses an adaptive expansion radius adjustment system, including:
[0041] Map width extraction module 201: used to calculate the overall width Wc of the passable area in the cost map in real time;
[0042] Vehicle body parameter interface module 202: used to obtain robot envelope dimensions Wv and Lv from the configuration library or URDF model;
[0043] Vehicle speed sensing module 203: used to collect and process the current speed v in real time;
[0044] Adaptive calculation unit 204: used to perform the method as described in S101-S106 to calculate the dynamic expansion radius R;
[0045] Map update interface module 205: Used to write the expansion radius R into the cost map expansion layer and notify the planner.
Claims
1. An adaptive expansion radius adjustment method based on a cost map, characterized in that, Includes the following steps: S1, real-time acquisition of the overall width Wc of the cost map; S2, obtain the geometric envelope dimensions of the vehicle body, including the vehicle width Wv and the vehicle length Lv; S3. Calculate the dynamic safety margin coefficient k(v) based on the real-time vehicle speed v, where k(v) is a monotonically increasing function of vehicle speed v. S4, considering the overall width Wc, vehicle dimensions Wv and Lv, and dynamic safety margin coefficient k(v), where α and β are empirical coefficients, R base It is the reference expansion radius, R max This is the maximum allowable expansion radius of the system. The adaptive expansion radius R is calculated using the following formula: S5, write the calculated expansion radius R into the expansion layer of the cost map, and trigger the path planning module to replan or locally correct the path.
2. The automatic navigation obstacle expansion radius dynamic adjustment method according to claim 1, characterized in that, The overall passage width Wc mentioned in S1 is the maximum continuous free space width extracted through grid map connectivity analysis or sliding window statistics.
3. The automatic navigation obstacle expansion radius dynamic adjustment method according to claim 1, characterized in that, The vehicle body geometric dimensions Wv and Lv mentioned in S2 are read in real time from the Robot Unified Parameter Configuration Library or URDF model file.
4. The automatic navigation obstacle expansion radius dynamic adjustment method according to claim 1, characterized in that, The real-time vehicle speed v mentioned in S3 is acquired through a wheel encoder, inertial measurement unit, or vehicle CAN bus interface.
5. The automatic navigation obstacle expansion radius dynamic adjustment method according to claim 1, characterized in that, The dynamic safety margin coefficient k(v) mentioned in S3 is defined by the following formula: Where vref is the preset speed reference value, and kmin and kma are the minimum and maximum values of the dynamic safety margin coefficient.
6. An adaptive expansion radius adjustment device, characterized in that, include: The system includes a map width extraction module, a vehicle parameter interface module, a vehicle speed sensing module, an adaptive calculation unit, and a map update interface module.
7. The adaptive expansion radius adjustment device according to claim 6, characterized in that, The map width extraction module is used to calculate the maximum passable channel width Wc in the local cost map where the robot is located in real time. Its specific configuration is as follows: The cost map grid is traversed using a sliding window statistical method or a ray scanning method; noise obstacles are identified and filtered through connected component analysis to extract continuous free space regions; and the maximum effective passage width of the robot's heading and perimeter is solved based on distance transformation or width histogram analysis. Output a real value Wc representing the degree of confinement of the environment to the adaptive computing unit.
8. The adaptive expansion radius adjustment device according to claim 6, characterized in that, The vehicle body parameter interface module is used to obtain the geometric dimensions of the robot body, read the predefined robot_width and robot_length parameters from the robot parameter server, and output the vehicle body width Wv and length Lv to the adaptive calculation unit.
9. The adaptive expansion radius adjustment device according to claim 6, characterized in that, The vehicle speed sensing module is used to collect the robot's real-time movement speed, subscribe to encoder or odometer topics in the ROS system, and obtain the instantaneous linear velocity v through differential calculation; the filtered real-time linear velocity v is output to the adaptive calculation unit.
10. The adaptive expansion radius adjustment device according to claim 6, characterized in that, The adaptive calculation unit is used to determine the optimal expansion radius R, and its execution process includes the following steps: S4-1: Based on the real-time vehicle speed v, the dynamic safety margin coefficient k(v) is calculated using the following nonlinear function: S4-2: Calculate the three types of radii based on the dynamic safety margin coefficient k(v) obtained in S3-1: S4-3: Calculate the final expansion radius R according to the following formula and output the result to the map update interface module.