A low-computational-load laser radar path planning and saving method and system
By using local environmental perception and obstacle simplification, obstacles are identified and simplified into geometric models, passable gaps are searched and transitional sub-targets are generated. Data is collected only in local fan-shaped areas, dynamic obstacles are detected in real time, and environmental feature maps are iteratively constructed. This solves the problem of high computational load in lidar path planning and achieves efficient and safe path planning.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- GUANGZHOU WEIKONG ROBOT CO LTD
- Filing Date
- 2026-01-19
- Publication Date
- 2026-06-23
AI Technical Summary
Existing lidar systems involve large amounts of data and a lot of invalid data during path planning, resulting in high computational demands. This makes them difficult to use on controllers with limited operational capabilities and prevents them from dynamically planning paths in unknown environments.
By using local environment perception and obstacle simplification, obstacles are identified and simplified into geometric models, passable gaps are searched and transitional sub-targets are generated, data is collected only in local fan-shaped areas, dynamic obstacles are detected in real time, and environmental feature maps are iteratively constructed.
It reduces the computational load of lidar path planning, improves the efficiency and applicability of path planning, and enables rapid response and safe movement in complex environments, adapting to dynamic changes.
Smart Images
Figure CN121558043B_ABST
Abstract
Description
Technical Field
[0001] This application relates to the field of lidar technology, and in particular to a method and system for lidar path planning and storage with low computational complexity. Background Technology
[0002] Existing LiDAR systems, when performing path planning, are constantly scanning, resulting in a large volume of output data. The presence of invalid data within this data places significant pressure on data transmission and processing, making them unsuitable for controllers with limited processing capabilities and greatly limiting the applicability of LiDAR. The sheer number of data points in LiDAR also leads to a much higher complexity in developing algorithms for point cloud segmentation, target classification, and other tasks compared to other algorithms, requiring substantial computation for path planning. Furthermore, most path planning algorithms require prior environmental mapping and path planning within a known environment, making them unsuitable for dynamic path planning in unknown environments. Summary of the Invention
[0003] This application provides a method and system for low-computation-load LiDAR path planning and storage, which reduces the computational load of LiDAR during path planning.
[0004] Firstly, a low-computation-load LiDAR path planning and storage method is provided, including the following steps: S1. Performing local environment perception and obstacle simplification, including: in the robot's current pose, controlling the robot to rotate to the direction aligned with the target point, and collecting LiDAR data in the fan-shaped area in front of the robot during the rotation; based on the distance change between adjacent LiDAR ranging points, identifying obstacles in the environment, and simplifying each identified obstacle into a geometric model with position and size parameters;
[0005] S2. Perform a passable gap search, including: calculating the angular relationship between the geometric model of each obstacle identified in step S1 and the direction of the target point, determining the obstacle closest to the direction of the target point; and determining whether there is a passable gap with a width greater than the width of the robot between the obstacle and the robot, or between the obstacle and an adjacent obstacle.
[0006] S3. Perform transition sub-target generation and local path planning, including: S31. If the passable gap exists, determine the center point of the gap or the safe point of the obstacle edge as the transition sub-target; S32. Plan the local motion path from the robot's current position to the transition sub-target; S33. During the movement towards the transition sub-target, only collect LiDAR data in a local fan-shaped area related to the direction of the local motion path, and detect and avoid dynamic or newly added obstacles not recorded in S1 in real time based on this local data;
[0007] S4. Perform iterative exploration and map building, including: when the robot reaches the transition sub-target, take that point as the new current position and repeat steps S1 to S3 until the final target point is reached; throughout the process, record all simplified obstacle model parameters and local motion paths that have been traversed, and form and update the environmental feature map.
[0008] In the above technical solution, S1. Local environment perception and obstacle simplification are performed, including: in the robot's current pose, controlling the robot to rotate to align with the target point, and collecting LiDAR data in the fan-shaped area in front of the robot during the rotation; based on the distance change between adjacent LiDAR ranging points, obstacles in the environment are identified, and each identified obstacle is simplified into a geometric model with position and size parameters; S2. Passable gap search is performed, including: calculating the angular relationship between the geometric model of each obstacle identified in step S1 and the direction of the target point, and determining the obstacle closest to the direction of the target point; determining whether there is a passable gap with a width greater than the robot's width between the obstacle and the robot, or between the obstacle and adjacent obstacles; S3. Transitional sub-target generation and local path planning are performed, including: S31. If the passable gap exists, the center point of the gap or the safe point of the obstacle's edge is determined as a transitional sub-target; S32. S33. Plan a local motion path from the robot's current position to the transition sub-target; S4. During the movement towards the transition sub-target, only collect LiDAR data within a local fan-shaped area related to the direction of the local motion path, and based on this local data, detect and avoid dynamic or newly added obstacles not recorded in S1 in real time; S5. Perform iterative exploration and map building, including: when the robot reaches the transition sub-target, take this point as the new current position, repeat steps S1 to S3 until the final target point is reached; throughout the process, record all simplified obstacle model parameters and the local motion paths already traversed, forming and updating the environmental feature map; reducing the computational load of LiDAR during path planning.
[0009] In one specific implementation scheme, step S1, "identifying obstacles based on the distance change between adjacent lidar ranging points," specifically includes:
[0010] S11. Convert the ranging values from the LiDAR into a vector point set with the robot as the origin; S12. Calculate the difference in distance between three consecutive adjacent points in the vector point set; S13. When the difference exceeds a set threshold, determine that point as an obstacle edge point; S14. Based on the consecutive obstacle edge points, fit the boundary of the obstacle and simplify the obstacle into a circular model, which consists of a center position (Xn, Yn) and a direction angle θ. nThe radius Rn is parameterized and denoted as obstacle. n (Xn, Yn, θ) n ,Rn).
[0011] In one specific implementation scheme, step S2, "determining whether a passable gap exists," specifically includes:
[0012] S21. Calculate the difference between the direction angle of each obstacle and the direction angle of the target point, and select the obstacle with the smallest difference as the closest obstacle; S22. If the vertical distance between the closest obstacle and the robot's current position is greater than the robot's width, then the gap is determined to be a passable gap; S23. Otherwise, calculate the distance between the closest obstacle and the boundary of the adjacent obstacle. If this distance is greater than the robot's width, then the gap is determined to be a passable gap.
[0013] In one specific implementation scheme, step S33, "real-time detection and avoidance of new obstacles based on local data," specifically includes:
[0014] When moving along a local path, if multiple consecutive laser ranging values are detected within the local fan-shaped area that are much smaller than the current distance to the transition sub-target, it is determined that there is a new obstacle ahead; the extension length of the new obstacle in the robot's lateral direction is calculated; the robot is controlled to perform an arc-shaped bypass operation with the obstacle's lateral length plus a safety margin as the radius.
[0015] In one specific implementation, during the arc-shaped detour, if the robot detects that it needs to continue parallel detour, it records the closest point of the obstacle detected by the lidar during the detour and controls the robot to move parallel to the line connecting that point until it completely passes the obstacle and then returns to the original local path.
[0016] In one specific implementation scheme, step S4, "forming and updating the environmental feature map," specifically involves: updating the simplified obstacle model recorded in each iteration. n (Xn, Yn, θ) n The displacement D of the local path (Rn) n With direction θp n The data is saved; when the robot runs again in the same environment, it can quickly locate and replan by matching the few currently perceived features with the saved environmental feature map.
[0017] Secondly, a low-computation-consumption lidar path planning and storage system is provided, including:
[0018] The data processing module is used to receive raw data from the lidar, perform the local environment perception and obstacle simplification steps, and output a list of simplified obstacle models.
[0019] The path decision module is used to receive target point information and the obstacle model list, execute the passable gap search and transition sub-target generation steps, and output transition sub-target and local path instructions.
[0020] The motion control and local obstacle avoidance module is used to control the robot's movement according to the local path instructions, and to perform the local data acquisition and real-time obstacle avoidance steps during the movement.
[0021] The map building and storage module is used to iteratively record the simplified obstacle model and the local paths that have been traversed, forming and updating the environmental feature map.
[0022] In the above technical solution, a data processing module is set up to receive raw data from the LiDAR, execute the local environment perception and obstacle simplification steps, and output a simplified obstacle model list; a path decision module is set up to receive target point information and the obstacle model list, execute the passable gap search and transition sub-target generation steps, and output transition sub-targets and local path instructions; a motion control and local obstacle avoidance module is set up to control the robot's movement according to the local path instructions, and execute the local data acquisition and real-time obstacle avoidance steps during the movement; a map building and storage module is set up to iteratively record the simplified obstacle models and the local paths that have been traversed, forming and updating the environmental feature map; thus reducing the computational load of the LiDAR during path planning.
[0023] In one possible implementation, the data processing module is configured in the obstacle simplification step to identify obstacle edges by calculating the vector distance difference between continuous lidar data points and to fit the obstacle to a circular model.
[0024] In one specific implementation, the motion control and local obstacle avoidance module is configured to subscribe to and process only LiDAR data within a specific angle range with the current direction of motion when controlling the robot to move toward the transition sub-target, and to perform real-time obstacle avoidance calculations based on this simplified data stream.
[0025] Thirdly, a computer-readable storage medium is provided, on which a computer program is stored, wherein when the computer program is executed by a processor, it implements the low-computation-load lidar path planning and saving method described in any one of the claims.
[0026] In the above technical solution, S1. Local environment perception and obstacle simplification are performed, including: in the robot's current pose, controlling the robot to rotate to align with the target point, and collecting LiDAR data in the fan-shaped area in front of the robot during the rotation; based on the distance change between adjacent LiDAR ranging points, obstacles in the environment are identified, and each identified obstacle is simplified into a geometric model with position and size parameters; S2. Passable gap search is performed, including: calculating the angular relationship between the geometric model of each obstacle identified in step S1 and the direction of the target point, and determining the obstacle closest to the direction of the target point; determining whether there is a passable gap with a width greater than the robot's width between the obstacle and the robot, or between the obstacle and adjacent obstacles; S3. Transitional sub-target generation and local path planning are performed, including: S31. If the passable gap exists, the center point of the gap or the safe point of the obstacle's edge is determined as a transitional sub-target; S32. S33. Plan a local motion path from the robot's current position to the transition sub-target; S4. During the movement towards the transition sub-target, only collect LiDAR data within a local fan-shaped area related to the direction of the local motion path, and based on this local data, detect and avoid dynamic or newly added obstacles not recorded in S1 in real time; S5. Perform iterative exploration and map building, including: when the robot reaches the transition sub-target, take this point as the new current position, repeat steps S1 to S3 until the final target point is reached; throughout the process, record all simplified obstacle model parameters and the local motion paths already traversed, forming and updating the environmental feature map; reducing the computational load of LiDAR during path planning. Attached Figure Description
[0027] Figure 1 A flowchart illustrating the low-computation-load lidar path planning and saving method provided in this application embodiment;
[0028] Figure 2 A structural block diagram of a low-computation-load lidar path planning and saving system provided in this application embodiment;
[0029] Figures 3a-3g This is a flowchart illustrating the low-computation-load lidar path planning and saving method provided in the embodiments of this application. Detailed Implementation
[0030] The present application will now be described in further detail with reference to the accompanying drawings and embodiments. Through these descriptions, the features and advantages of the present application will become clearer and more apparent.
[0031] The term “exemplary” as used herein means “serving as an example, embodiment, or illustration.” Any embodiment illustrated herein as “exemplary” is not necessarily to be construed as superior to or better than other embodiments. Although various aspects of embodiments are shown in the accompanying drawings, the drawings are not necessarily drawn to scale unless specifically indicated otherwise.
[0032] Furthermore, the technical features involved in the different embodiments of this application described below can be combined with each other as long as they do not conflict with each other.
[0033] To facilitate understanding of the low-computation-load LiDAR path planning and storage method and system provided in this application embodiment, its application scenario is first explained. The low-computation-load LiDAR path planning and storage method and system provided in this application embodiment are used to reduce the computational load of LiDAR during path planning. Existing LiDARs, when performing path planning, are constantly scanning, resulting in a large amount of output data. Furthermore, the presence of invalid data within this data puts significant pressure on data transmission and processing, making it unsuitable for controllers with limited processing capabilities and greatly reducing the applicability of LiDARs. The large number of data points in LiDARs also leads to a much higher complexity in developing algorithms for point cloud segmentation, target classification, etc., requiring substantial computation for path planning. Secondly, most path planning algorithms require prior environmental mapping and path planning within a known environment, making them unsuitable for dynamic path planning in unknown environments. Therefore, this application embodiment provides a low-computation-load LiDAR path planning and storage method and system to reduce the computational load of LiDARs during path planning. The following detailed description, in conjunction with specific accompanying drawings, illustrates this method and system in practice.
[0034] refer to Figure 1 and Figure 2 , Figure 1 A flowchart illustrating the low-computation-load lidar path planning and saving method provided in this application embodiment; Figure 2 A structural block diagram of a low-computation-load lidar path planning and saving system provided in this application embodiment; Figures 3a-3g This is a flowchart illustrating the low-computation-load lidar path planning and saving method provided in the embodiments of this application.
[0035] exist Figure 1In this application embodiment, a low-computation-load LiDAR path planning and saving method is provided, including the following steps: S1. Perform local environment perception and obstacle simplification, including: in the robot's current pose, control the robot to rotate to the direction aligned with the target point, and collect LiDAR data in the fan-shaped area in front of the robot during the rotation; based on the distance change between adjacent LiDAR ranging points, identify obstacles in the environment, and simplify each identified obstacle into a geometric model with position and size parameters;
[0036] S2. Perform a passable gap search, including: calculating the angular relationship between the geometric model of each obstacle identified in step S1 and the direction of the target point, determining the obstacle closest to the direction of the target point; and determining whether there is a passable gap with a width greater than the width of the robot between the obstacle and the robot, or between the obstacle and an adjacent obstacle.
[0037] S3. Perform transition sub-target generation and local path planning, including: S31. If the passable gap exists, determine the center point of the gap or the safe point of the obstacle edge as the transition sub-target; S32. Plan the local motion path from the robot's current position to the transition sub-target; S33. During the movement towards the transition sub-target, only collect LiDAR data in a local fan-shaped area related to the direction of the local motion path, and detect and avoid dynamic or newly added obstacles not recorded in S1 in real time based on this local data;
[0038] S4. Perform iterative exploration and map building, including: when the robot reaches the transition sub-target, take that point as the new current position and repeat steps S1 to S3 until the final target point is reached; throughout the process, record all simplified obstacle model parameters and local motion paths that have been traversed, and form and update the environmental feature map.
[0039] In the above technical solution, S1. Local environment perception and obstacle simplification are performed, including: in the robot's current pose, controlling the robot to rotate to align with the target point, and collecting LiDAR data in the fan-shaped area in front of the robot during the rotation; based on the distance change between adjacent LiDAR ranging points, obstacles in the environment are identified, and each identified obstacle is simplified into a geometric model with position and size parameters; S2. Passable gap search is performed, including: calculating the angular relationship between the geometric model of each obstacle identified in step S1 and the direction of the target point, and determining the obstacle closest to the direction of the target point; determining whether there is a passable gap with a width greater than the robot's width between the obstacle and the robot, or between the obstacle and adjacent obstacles; S3. Transitional sub-target generation and local path planning are performed, including: S31. If the passable gap exists, the center point of the gap or the safe point of the obstacle's edge is determined as a transitional sub-target; S32. S33. Plan a local motion path from the robot's current position to the transition sub-target; S4. During the movement towards the transition sub-target, only collect LiDAR data within a local fan-shaped area related to the direction of the local motion path, and based on this local data, detect and avoid dynamic or newly added obstacles not recorded in S1 in real time; S5. Perform iterative exploration and map building, including: when the robot reaches the transition sub-target, take this point as the new current position, repeat steps S1 to S3 until the final target point is reached; throughout the process, record all simplified obstacle model parameters and the local motion paths already traversed, forming and updating the environmental feature map; reducing the computational load of LiDAR during path planning.
[0040] Specifically, the beneficial effects include:
[0041] Reduced computational load and improved efficiency: This method, through local environmental perception and obstacle simplification, only collects LiDAR data within a fan-shaped area in front of the robot and identifies obstacles based on changes in distance between adjacent ranging points, simplifying them into geometric models. Compared to comprehensively collecting and processing all LiDAR data, this significantly reduces the amount of data processing. During movement towards the transitional sub-target, only data from a local fan-shaped area related to the local movement path direction is collected, further reducing computational load and making the path planning process more efficient. This allows for rapid response to environmental changes and improves the robot's operational efficiency in complex environments.
[0042] Precise identification of passable gaps: By calculating the angular relationship between the obstacle's geometric model and the target point's direction, the system identifies the obstacle closest to the target point and determines whether there is a passable gap between it and the robot or adjacent obstacles with a width greater than the robot's width. This precise judgment method accurately finds a feasible path for the robot, avoiding collisions or detours caused by inaccurate judgment of passable areas, ensuring the robot's safe and smooth progress towards the target point.
[0043] Flexible generation of transitional sub-objectives and planned paths: When a passable gap exists, the center point of the gap or a safe point on the edge of the obstacle is identified as a transitional sub-objective, and a local motion path from the current position to the transitional sub-objective is planned. This flexible method of generating transitional sub-objectives can dynamically adjust the path planning according to the actual environmental conditions, enabling the robot to better adapt to complex and changing environments and improving the rationality and feasibility of path planning.
[0044] Effective handling of dynamic obstacles: During movement towards the transitional sub-target, the robot detects and avoids dynamic or newly added obstacles not recorded in the initial stage in real time based on LiDAR data within a local fan-shaped area. This feature enables the robot to respond promptly to unexpected situations in the environment, enhancing its adaptability to dynamic environments and ensuring the robot's safety during movement.
[0045] Constructing a complete environmental feature map: During iterative exploration, all simplified obstacle model parameters and traversed local motion paths are recorded to form and update the environmental feature map. This map provides rich environmental information for the robot's subsequent actions, helping it better understand its surroundings, further optimize path planning, and improve overall task execution capabilities.
[0046] In one specific implementation scheme, step S1, "identifying obstacles based on the distance change between adjacent lidar ranging points," specifically includes:
[0047] S11. Convert the ranging values from the LiDAR into a vector point set with the robot as the origin; S12. Calculate the difference in distance between three consecutive adjacent points in the vector point set; S13. When the difference exceeds a set threshold, determine that point as an obstacle edge point; S14. Based on the consecutive obstacle edge points, fit the boundary of the obstacle and simplify the obstacle into a circular model, which consists of a center position (Xn, Yn) and a direction angle θ. n The radius Rn is parameterized and denoted as obstacle. n (Xn, Yn, θ) n ,Rn).
[0048] In one specific implementation scheme, step S2, "determining whether a passable gap exists," specifically includes:
[0049] S21. Calculate the difference between the direction angle of each obstacle and the direction angle of the target point, and select the obstacle with the smallest difference as the closest obstacle; S22. If the vertical distance between the closest obstacle and the robot's current position is greater than the robot's width, then the gap is determined to be a passable gap; S23. Otherwise, calculate the distance between the closest obstacle and the boundary of the adjacent obstacle. If this distance is greater than the robot's width, then the gap is determined to be a passable gap.
[0050] In one specific implementation scheme, step S33, "real-time detection and avoidance of new obstacles based on local data," specifically includes:
[0051] When moving along a local path, if multiple consecutive laser ranging values are detected within the local fan-shaped area that are much smaller than the current distance to the transition sub-target, it is determined that there is a new obstacle ahead; the extension length of the new obstacle in the robot's lateral direction is calculated; the robot is controlled to perform an arc-shaped bypass operation with the obstacle's lateral length plus a safety margin as the radius.
[0052] In one specific implementation, during the arc-shaped detour, if the robot detects that it needs to continue parallel detour, it records the closest point of the obstacle detected by the lidar during the detour and controls the robot to move parallel to the line connecting that point until it completely passes the obstacle and then returns to the original local path.
[0053] In one specific implementation scheme, step S4, "forming and updating the environmental feature map," specifically involves: updating the simplified obstacle model recorded in each iteration. n (Xn, Yn, θ) n The displacement D of the local path (Rn) n With direction θp n The data is saved; when the robot runs again in the same environment, it can quickly locate and replan by matching the few currently perceived features with the saved environmental feature map.
[0054] In one specific implementation scheme, the low-computation-load LiDAR path planning and saving method addresses the shortcomings of LiDAR in path planning by simplifying complex environments. Unnecessary exploration areas do not require path planning, allowing the robot to quickly find movable paths, avoid fixed obstacles, and rapidly reach the designated target location. During movement, the number, area, and location information of obstacles encountered are recorded, achieving the advantage of recording while moving. Simultaneously, the algorithm of this application is used for planning, employing a low-computation method to ensure that even controllers with low computational requirements can use it, greatly expanding the application scenarios of this method.
[0055] Assume the robot's maximum lateral distance is L. robot The maximum detection value of the lidar is S max The positive X-axis direction is directly in front of the robot, and the positive Y-axis direction is directly to its left. The robot's initial position and orientation are (P... x0 P y0 The position and orientation of the first target point are (P, θ0). x1 P y1 ,θ1), the reference schematic map is as follows Figure 3a As shown.
[0056] The low-computation-load lidar path planning and storage method includes the following steps:
[0057] The first step is to power on the robot and keep it stationary, setting this position as the initial position and orientation (P). x0 P y0 ,θ0), calculate the distance d and the included angle θ between the first target point and the target at the initial position. The formula for calculating the distance d is: , The formula for calculating the included angle θ is: Starting from this position, control the robot to rotate by an angle a of θ radians (radians = a × π / 180). Record all LiDAR detection values (L0, L1, L2... L...) in front of the robot during this movement (only 120° is considered). n Record it, such as Figure 3b As shown.
[0058] Use the formula: , Convert to vector combination: The vector P of the lidar detection point is: , , .
[0059] The second step is to set the vector of the first LiDAR detection point as P1, and so on for the other vector numbers. Then, begin calculating the vector relationships between three adjacent points, such as the distance d from the second point P2 to P1. P2P1 The distance d between the third point P3 and P2 P3P2 The distance d between the fourth point P4 and P3 P4P3 The difference, that is (d P3P2 -d P2P1 ), (d P4P3 -d P3P21If the two differences are small, it means that the three lidar points are continuous, and the calculation continues. If a large difference appears, it means that the lidar has detected a breakpoint, which is used as a reference, and the differences of adjacent subsequent points need to be considered. If the adjacent differences are not significant, and the detection values L of these lidars are all less than the maximum detection value S of the lidars. max This indicates that the location is a wall or a relatively large obstacle; if the difference between adjacent values is large, and the detection values L of these lidars are all greater than the maximum detection value S of the lidars. max This indicates that the obstacle is fixed. By calculating the point of greatest change in adjacent values, the edge trajectory of the obstacle can be calculated. By calculating the length of the edge trajectory, the obstacle can be treated as a circular obstacle (simplified obstacle). Combined with the vector calculation of the initial point, the relative position and relative size of the obstacle can finally be obtained, i.e., the obstacle. n (X) n Y n θ n R n ),like Figure 3c As shown.
[0060] Third, after the second step, the obstacle information within the robot's rotation range can be recorded. At this point, the coordinates of the initial point and the first target point are combined to form a vector P. target1 Calculate obstacle n θ and P target1 After finding the closest obstacle θ, the cross product of the two obstacles is used to calculate whether the vertical distance to the robot's front is sufficient for the robot to pass; that is, the vertical distance needs to be greater than L. robot If the value is greater than this, it means the robot can move along this path, and this calculated point is taken as the transition target point P. transititarget If it is less than L, then search for adjacent obstacles in the opposite direction, calculate the boundary distance between the obstacle and its adjacent obstacles, and check if it is greater than L. robot If the distance is greater than the center point, it means the distance between the two obstacles is sufficient for the robot to pass. In this case, the distance between the boundary of the two adjacent obstacles and the center point is taken as the transition target point P. transititarget If the distance is still less than the threshold, it means the distance between these two obstacles is insufficient for the robot to pass. In this case, the boundary distance of the next adjacent obstacle needs to be calculated until a sufficient boundary distance is found for the robot to pass through. The center point of the boundary distance is then used as the transition target point P. transititarget ,like Figure 3d As shown.
[0061] If the distances between all obstacle boundaries are ultimately insufficient for the robot to pass, it means there is no suitable path within the θ-radian range of rotation in step one. In this case, it is necessary to continue rotating by θ radians (i.e., within the range of 2θ radians) based on that θ radian, and repeat steps one through three until a suitable transition target point P that can be passed is found. transititarget If the search is still unsuccessful after three repetitions, it means that our initial first target point is unreachable and we need to change to a second target point.
[0062] The fourth step was to determine the transition target point P. transititarget Next, it is necessary to calculate the trajectory from the initial point to the transition target point. The algorithm used is as follows: Assume the transition target point P. transititarget The nearby obstacles are obstacle2 (X2, Y2, θ2, R2) and obstacle3 (X3, Y3, θ3, R3). First, find the center points of obstacle2 and obstacle3: , Then, the path from the initial point to the center point is (distance D, angle θp), where, , At this point, the robot first saves the values of D and θp for this movement, thus saving the trajectory of the movement path. Finally, the robot can move according to the values of D and θp, and reach the transition target point P. transititarget During this period, the robot only needs to collect LiDAR data within the range of θp±10°; all other data does not need to be collected. Figure 3e As shown.
[0063] In special circumstances, if several consecutive lidar detection values within this range are significantly smaller than the distance from the robot itself to the transition target point P, then... transititarget The value indicates that there is an obstacle ahead. When there is an obstacle, the lateral length of the obstacle can be calculated by calculating the tangent distance formed by the continuous LiDAR detection points. The robot only needs to add 1.5 times the robot's lateral distance L to this lateral length. robot (Leave a safe distance to avoid collisions) A semi-circular motion with a radius of [radius] can bypass the obstacle. If an obstacle is still found while bypassing it, it indicates that the obstacle is a long, narrow object. In this case, the robot should use the farthest point on the side of the obstacle detected by the LiDAR during the bypass (i.e., the closest distance the LiDAR detects the obstacle) as a reference point and move parallel to that point until the robot detects that the obstacle has been completely passed. Then, the robot returns to the planned initial path and continues moving until it reaches the transition target point P. transititarget The movement stops within the specified range, awaiting the next calculation.
[0064] Step 5: Reach the transition target point P transititarget Then, this point will be used as the starting point for the next path exploration, such as... Figure 3f As shown.
[0065] By repeating steps one through four, a series of obstacle information can be obtained. n (X) n Y n θ n R n )) and a series of path trajectories (distance D) n Angle θp n Once the robot reaches the designated target point (e.g., the first target point), all obstacles and path trajectories encountered during the entire process can be calculated. This provides a clear understanding of all information within the operating area, which is then saved to record the robot's movement within that area. When the robot moves within this area, its current position can be compared to its initial path exploration position to calculate its relative position within the area. This also allows for quick identification of obstacles without complex calculations, enabling path exploration based on previously obtained information. If new obstacles or new runnable paths are discovered during path exploration, this information can be added to the previously saved data for easier robot calculations, better path exploration, and data saving. Figure 3g As shown in the image. By following the steps above, you can complete the path exploration and save the data.
[0066] This application addresses the application scenarios of LiDAR in path exploration and saving by performing mathematical modeling operations on obstacles and movable paths. It calculates the mathematical patterns of obstacles and movement paths at each stage, and combines these related mathematical patterns to perform movement and saving operations. This yields the characteristic parameters of these environments, which the robot can use to perform mathematical calculations. This allows the robot to quickly locate its relative position in the environment and achieve faster path exploration and saving in the next iteration.
[0067] In this embodiment, the beneficial effects include:
[0068] Taking into full account the problems of LiDAR in path exploration and saving, the algorithm and logic are used to optimize the redundant and invalid parts of LiDAR data. This allows the path exploration and saving to be completed by using only the correct and effective detection points. The algorithm can quickly find the most suitable path for movement through calculation, and finally achieve the desired path exploration and saving effect.
[0069] After processing by this method, the amount of data processed by the LiDAR is greatly reduced, solving the problem that controllers with low computational requirements can also handle path exploration and saving functions well. It greatly reduces the number of LiDAR detection points required, reduces computational power consumption, increases the robustness of path exploration and saving, and improves the applicability of LiDAR for application scenarios that no longer require high-speed performance.
[0070] exist Figure 2 In this application, an embodiment provides a low-computation-load lidar path planning and storage system, including:
[0071] The data processing module is used to receive raw data from the lidar, perform the local environment perception and obstacle simplification steps, and output a list of simplified obstacle models.
[0072] The path decision module is used to receive target point information and the obstacle model list, execute the passable gap search and transition sub-target generation steps, and output transition sub-target and local path instructions.
[0073] The motion control and local obstacle avoidance module is used to control the robot's movement according to the local path instructions, and to perform the local data acquisition and real-time obstacle avoidance steps during the movement.
[0074] The map building and storage module is used to iteratively record the simplified obstacle model and the local paths that have been traversed, forming and updating the environmental feature map.
[0075] In the above technical solution, a data processing module is set up to receive raw data from the LiDAR, execute the local environment perception and obstacle simplification steps, and output a simplified obstacle model list; a path decision module is set up to receive target point information and the obstacle model list, execute the passable gap search and transition sub-target generation steps, and output transition sub-targets and local path instructions; a motion control and local obstacle avoidance module is set up to control the robot's movement according to the local path instructions, and execute the local data acquisition and real-time obstacle avoidance steps during the movement; a map building and storage module is set up to iteratively record the simplified obstacle models and the local paths that have been traversed, forming and updating the environmental feature map; thus reducing the computational load of the LiDAR during path planning.
[0076] In one possible implementation, the data processing module is configured in the obstacle simplification step to identify obstacle edges by calculating the vector distance difference between continuous lidar data points and to fit the obstacle to a circular model.
[0077] In one specific implementation, the motion control and local obstacle avoidance module is configured to subscribe to and process only LiDAR data within a specific angle range with the current direction of motion when controlling the robot to move toward the transition sub-target, and to perform real-time obstacle avoidance calculations based on this simplified data stream.
[0078] This application also provides a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the low-computation-load lidar path planning and saving method described in any of the claims.
[0079] In the above technical solution, S1. Local environment perception and obstacle simplification are performed, including: in the robot's current pose, controlling the robot to rotate to align with the target point, and collecting LiDAR data in the fan-shaped area in front of the robot during the rotation; based on the distance change between adjacent LiDAR ranging points, obstacles in the environment are identified, and each identified obstacle is simplified into a geometric model with position and size parameters; S2. Passable gap search is performed, including: calculating the angular relationship between the geometric model of each obstacle identified in step S1 and the direction of the target point, and determining the obstacle closest to the direction of the target point; determining whether there is a passable gap with a width greater than the robot's width between the obstacle and the robot, or between the obstacle and adjacent obstacles; S3. Transitional sub-target generation and local path planning are performed, including: S31. If the passable gap exists, the center point of the gap or the safe point of the obstacle's edge is determined as a transitional sub-target; S32. S33. Plan a local motion path from the robot's current position to the transition sub-target; S4. During the movement towards the transition sub-target, only collect LiDAR data within a local fan-shaped area related to the direction of the local motion path, and based on this local data, detect and avoid dynamic or newly added obstacles not recorded in S1 in real time; S5. Perform iterative exploration and map building, including: when the robot reaches the transition sub-target, take this point as the new current position, repeat steps S1 to S3 until the final target point is reached; throughout the process, record all simplified obstacle model parameters and the local motion paths already traversed, forming and updating the environmental feature map; reducing the computational load of LiDAR during path planning.
[0080] Those skilled in the art will know that this application can be implemented as a system, method, or computer program product.
[0081] Therefore, this disclosure can be implemented in the following forms: it can be entirely hardware, entirely software (including firmware, resident software, microcode, etc.), or a combination of hardware and software, generally referred to herein as a "circuit," "module," or "system." Furthermore, in some embodiments, this application can also be implemented as a computer program product in one or more computer-readable media, the computer-readable media containing computer-readable program code.
[0082] Any combination of one or more computer-readable media may be used. A computer-readable medium can be a computer-readable signal medium or a computer-readable storage medium. A computer-readable storage medium can be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples (a non-exhaustive list) of computer-readable storage media include: an electrical connection having one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof. In this document, a computer-readable storage medium can be any tangible medium that contains or stores a program that can be used by or in connection with an instruction execution system, apparatus, or device.
[0083] Although embodiments of this application have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting this application. Those skilled in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of this application. Based on this, various substitutions and improvements can be made to this application, all of which fall within the protection scope of this application.
Claims
1. A low-computational-load laser radar path planning and saving method, characterized by, Includes the following steps: S1. Perform local environmental perception and obstacle simplification, including: in the robot's current pose, control the robot to rotate to the direction aligned with the target point, and collect LiDAR data in the fan-shaped area in front of the robot during the rotation; based on the distance change between adjacent LiDAR ranging points, identify obstacles in the environment, and simplify each identified obstacle into a geometric model with position and size parameters; S2. Perform a passable gap search, including: calculating the angular relationship between the geometric model of each obstacle identified in step S1 and the direction of the target point, determining the obstacle closest to the direction of the target point; and determining whether there is a passable gap with a width greater than the width of the robot between the obstacle and the robot, or between the obstacle and an adjacent obstacle. S3. Perform transition sub-target generation and local path planning, including: S31. If the passable gap exists, determine the center point of the gap or the safe point of the obstacle edge as the transition sub-target; S32. Plan the local motion path from the robot's current position to the transition sub-target; S33. During the movement towards the transition sub-target, only collect LiDAR data in a local fan-shaped area related to the direction of the local motion path, and detect and avoid dynamic or newly added obstacles not recorded in S1 in real time based on this local data; S4. Perform iterative exploration and map building, including: when the robot reaches the transition sub-target, take the transition sub-target as the new current position, repeat steps S1 to S3 until the final target point is reached; throughout the process, record all simplified obstacle model parameters and local motion paths that have been traversed, and form and update the environmental feature map.
2. The low-computation-load lidar path planning and storage method according to claim 1, characterized in that, Step S1, "Identifying obstacles based on distance changes between adjacent lidar ranging points," specifically includes: S11. converting the ranging values of the laser radar into a set of vector points with the robot as the origin; S12. calculating the difference of the distance between three consecutive adjacent points in the set of vector points; S13. when the difference exceeds a set threshold, determining that the ranging point of the laser radar is an obstacle edge point; S14. fitting the boundary of the obstacle according to the consecutive obstacle edge points, and simplifying the obstacle into a circular model, which is parameterized by the center position (Xn, Yn), the direction angle θ n and the radius Rn, denoted as obstacle n (Xn, Yn, θ n , Rn).
3. The low-computation-load lidar path planning and storage method according to claim 2, characterized in that, Step S2, "determining whether a passable gap exists," specifically includes: S21. Calculate the difference between the direction angle of each obstacle and the direction angle of the target point, and select the obstacle with the smallest difference as the closest obstacle; S22. If the vertical distance between the closest obstacle and the robot's current position is greater than the robot's width, then the gap is determined to be a passable gap; S23. Otherwise, calculate the distance between the closest obstacle and the boundary of the adjacent obstacle. If this distance is greater than the robot's width, then the gap is determined to be a passable gap.
4. The low-computation-load lidar path planning and storage method according to claim 3, characterized in that, Step S33, "real-time detection and avoidance of new obstacles based on local data," specifically includes: When moving along a local path, if multiple consecutive laser ranging values are detected within the local fan-shaped area that are much smaller than the current distance to the transition sub-target, it is determined that there is a new obstacle ahead; the extension length of the new obstacle in the robot's lateral direction is calculated; the robot is controlled to perform an arc-shaped bypass operation with the obstacle's lateral length plus a safety margin as the radius.
5. The low-computation-load lidar path planning and storage method according to claim 4, characterized in that, During the arc-shaped detour, if the robot detects that it needs to continue parallel detour, it records the closest point of the obstacle detected by the lidar during the detour and controls the robot to move parallel to the line connecting the closest point and the lidar until it completely passes the obstacle and then returns to the original local path.
6. The low-computation-load lidar path planning and storage method according to claim 5, characterized in that, The "forming and updating the environment feature map" in step S4 specifically refers to: saving the simplified obstacle model obstacle n (Xn, Yn, θ n , Rn) and the displacement D n of the local path with the direction θp n recorded in each iteration; when the robot runs in the same environment again, the quick positioning and re-planning are performed by matching the current perceived few features with the saved environment feature map.
7. A low-computation-load lidar path planning and storage system, characterized in that, include: The data processing module is used to receive raw data from the LiDAR, perform local environment perception and obstacle simplification steps, and output a list of simplified obstacle models. The path decision module is used to receive target point information and the obstacle model list, perform passable gap search and transition sub-target generation steps, and output transition sub-target and local path instructions. The motion control and local obstacle avoidance module is used to control the robot's movement according to the local path instructions, and to perform local data acquisition and real-time obstacle avoidance steps during the movement. The local data acquisition and real-time obstacle avoidance steps include: planning a local motion path from the robot's current position to the transition sub-target; during the movement to the transition sub-target, only collecting LiDAR data in a local fan-shaped area related to the direction of the local motion path, and detecting and avoiding dynamic or newly added obstacles in real time based on this local data. The map building and storage module is used to iteratively record the simplified obstacle model and the local paths that have been traversed, forming and updating the environmental feature map.
8. The low-computation-load lidar path planning and storage system according to claim 7, characterized in that, In the obstacle simplification step, the data processing module is configured to identify obstacle edges by calculating the vector distance difference between continuous lidar data points and fit the obstacle into a circular model.
9. The low-computation-load lidar path planning and storage system according to claim 8, characterized in that, When controlling the robot to move toward the transition sub-target, the motion control and local obstacle avoidance module is configured to subscribe to and process only LiDAR data within a specific angle range with the current direction of movement, and to perform real-time obstacle avoidance calculations based on this simplified data stream.
10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the low-computation-load lidar path planning and saving method as described in any one of claims 1-6.