Layered collision detection and path search method for sweeper structure

Abstract

The invention relates to a layered collision detection and path search method for a sweeper structure. According to the method, based on the vehicle body structure characteristic that the sweeper is wide in the upper portion and narrow in the lower portion, environment point cloud data are subjected to hierarchical processing according to height, and at least low-layer obstacles and high-layer obstacles are distinguished; and determining the width difference of the vehicle in the high-layer space and the low-layer space in combination with the vehicle equivalent widths corresponding to different heights, and implementing differential expansion processing on the obstacles with different heights in the grid map. On the basis of completing layered expansion, a unified layered Voronoi diagram is constructed, the layered Voronoi diagram is introduced into a path search process, and the driving path of the sweeper is constrained or guided, so that the effective passing space below the blind compensation radar is fully utilized on the premise of ensuring the obstacle avoidance safety, and the blind compensation efficiency is improved. And the traffic capacity and the path planning efficiency of the sweeper in a complex and narrow scene are improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of automatic driving, in particular to a layered collision detection and path search method for a sweeping vehicle structure. BACKGROUND

[0002] With the application of unmanned driving technology in the field of environmental sanitation, unmanned sweeping vehicles are gradually deployed in open and unstructured environments such as sidewalks, auxiliary roads, parks and villages in cities. Compared with standard road scenes, such scenes are narrow in space, complex in obstacle shape, and dense in dynamic targets such as pedestrians, bicycles and electric vehicles, which puts higher requirements on the collision detection capability and path planning accuracy of the vehicle.

[0003] The collision detection and path planning methods of existing autonomous vehicles are mostly based on the assumption that the vehicle has a consistent shape in the height direction, usually using a fixed vehicle width to uniformly inflate obstacles, and generating a grid map or a Voronoi diagram for path search on this basis. However, in unmanned sweeping vehicles, in order to eliminate the perception blind area and improve the running safety, the blind filling radar is often installed on the upper part of the vehicle body and extends outward, making the vehicle as a whole present an irregular structure of being wide at the top and narrow at the bottom. The above uniform inflation strategy fails to reflect the actual passing boundary of the vehicle at different heights, which easily leads to the overcompression of the passable space, thereby reducing the passing ability of the vehicle in narrow areas.

[0004] For the problem of obstacle avoidance in complex environments, existing technologies propose layered collision detection or multi-layer obstacle modeling methods to distinguish the influence of obstacles at different heights on vehicle safety. However, most of the existing solutions remain at the perception or grid construction level, and do not effectively integrate layered information into the generation process of global path structures such as Voronoi diagrams. In the path planning stage, it is still difficult to distinguish the passing constraints corresponding to obstacles at different heights, leading to the difficulty of balancing safety and passing efficiency in the search results.

[0005] Therefore, how to combine the real vehicle body structure characteristics of the sweeping vehicle being wide at the top and narrow at the bottom, and directly introduce the collision constraints at different heights into the construction process of the path planning basic structure, while ensuring obstacle avoidance safety and improving the passing ability in complex and narrow scenes, has become a technical problem that needs to be solved in the field.

[0006] Therefore, the existing technology still needs to be improved. SUMMARY

[0007] In view of the above deficiencies of the prior art, the path planning method of the existing cleaning vehicle in a complex and narrow scene has not fully considered the structural differences existing in the height direction of the vehicle, especially has failed to effectively introduce the passing constraints under different heights into the generation process of the path planning basic structure such as the Voronoi diagram, resulting in that the path search result is difficult to balance between safety and passing efficiency. Therefore, it is necessary to propose a new technical scheme, which, on the basis of taking into account the real vehicle body structure characteristics of the cleaning vehicle, realizes reasonable modeling of different height obstacles, and directly serves the path search process with the collision constraints formed thereby, so as to improve the passing ability and running safety of the cleaning vehicle in a complex scene.

[0008] The technical scheme of the present application is as follows: The present application provides a layered collision detection and path search method for a cleaning vehicle structure, comprising the following steps: Obtain point cloud data of the environment around the cleaning vehicle, and according to the distribution characteristics of the obstacles in the height direction, perform height layering on the point cloud data, and at least distinguish low-layer obstacles and high-layer obstacles; Based on the vehicle body structure of the cleaning vehicle which is wide at the top and narrow at the bottom, establish a vehicle equivalent width model corresponding to different heights, and determine the width difference between the vehicle equivalent width corresponding to the high layer and the vehicle equivalent width corresponding to the low layer; Map the point cloud data after height layering to generate a grid map, and according to the width difference, perform inflation processing on the grid area corresponding to the high-layer obstacles, and perform inflation processing less than the high layer or no inflation processing on the grid area corresponding to the low-layer obstacles; Based on the grid map after differential inflation processing, construct a unified layered Voronoi diagram; In the path search process, introduce the layered Voronoi diagram as path constraint or search guide information to plan the driving path of the cleaning vehicle, so as to improve the passing ability in a complex and narrow scene under the premise of ensuring obstacle avoidance safety.

[0009] In one embodiment, the height layering comprises: Based on the spatial density and height information of the point cloud, the point cloud is clustered and segmented, and the height range of each point cloud cluster in the clustering result is determined to determine the obstacle level to which it belongs.

[0010] In one embodiment, the clustering and segmentation is realized by using the DBSCAN algorithm.

[0011] In one embodiment, the vehicle equivalent width model is established based on the installation height of the blind filling radar of the cleaning vehicle and its extension position.

[0012] In one embodiment, the width difference is the difference between the vehicle body width of the cleaning vehicle in the state containing the blind filling radar and the vehicle body width of the cleaning vehicle in the state not containing the blind filling radar.

[0013] In one embodiment, the grid area corresponding to the high-layer obstacle is expanded with the expansion radius determined based on the width difference.

[0014] In one embodiment, the grid area corresponding to the low-layer obstacle is not expanded or is expanded with an expansion radius smaller than that of the high-layer obstacle, to allow the sweeper to pass through the space below the blind-filling radar.

[0015] In one embodiment, the hierarchical Voronoi diagram is generated by performing Delaunay triangulation on the grid map after the differential expansion.

[0016] In one embodiment, the path search adopts a hybrid A* algorithm and combines the hierarchical Voronoi diagram for node expansion or path evaluation during the search process.

[0017] In one embodiment, the distance from the path node to the edge of the hierarchical Voronoi diagram is introduced as an evaluation parameter in the cost function or heuristic function of the hybrid A* algorithm.

[0018] In summary, to solve the problems of insufficient use of passing space, difficulty in balancing safety and efficiency, etc. in the path planning process of existing sweepers in complex narrow scenes, the present application breaks through the assumption of consistent vehicle shape in the height direction in traditional path planning, introduces the passing constraints of the vehicle at different heights into the construction process of the path planning infrastructure. By performing hierarchical processing on the environment perception data and combining the structure characteristics of the sweeper with wide upper and narrow lower, the vehicle equivalent width model corresponding to different heights is established, differential expansion is performed on obstacles at different heights, and a unified hierarchical Voronoi diagram is generated, so that the path search stage can directly perceive the actual passing space corresponding to obstacles at different heights.

[0019] Compared with the existing technical solution that only introduces hierarchical collision detection in the perception or post-processing stage, the present application prepositions and deeply integrates hierarchical information into the path planning process, so that the path search can reasonably use the low-layer effective passing space while ensuring the safety of the blind-filling radar, thereby significantly improving the passing ability and overall running efficiency of the sweeper in complex scenes such as narrow lanes and half-obstructions. The technical solution of the present application has clear structure, complete logic, good engineering realizability, and is suitable for automatic driving devices with irregular vehicle body structure such as unmanned sweepers.

[0020] Compared with the prior art, the present application has at least the following unexpected beneficial effects: Firstly, the present application does not follow the modeling assumption of "vehicle shape consistent in height direction" in existing path planning, but starts from the real vehicle structure of the sweeper, which is wide at the top and narrow at the bottom, and directly introduces the equivalent width of the vehicle at different heights into the obstacle modeling and V-Graph generation process. Through this structure-driven layered modeling method, the path planning stage can distinguish the corresponding passing constraints of obstacles at different heights, thereby releasing the effective passing space at the lower layer which has been "covered up" by the overall inflation strategy in the traditional method without reducing safety, and this effect cannot be obtained by simply adjusting the inflation parameter.

[0021] Secondly, the present application prepositions and deeply integrates the layered collision detection results into the construction stage of the V-Graph, rather than using it only as an additional constraint after perception or path search, so that the V-Graph itself has a high degree of perception. The path structure generated in this way reflects the spatial influence of obstacles at different heights at the beginning of the search, avoiding the passive planning mode of "generating a path first and then repeatedly modifying" in the prior art, and significantly reducing the number of search backtracks and invalid expansion nodes in complex and narrow scenes. This effect is difficult to achieve in traditional V-Graph path planning.

[0022] Thirdly, by determining the differential inflation strategy of high and low layer obstacles based on the difference in vehicle structure, the present application forms a self-consistent balance mechanism between the safety obstacle avoidance requirement of the blind filling radar and the passing ability of the vehicle. That is, while ensuring that the safety distance in the corresponding height range of the blind filling radar is not weakened, the vehicle is allowed to pass reasonably in the space below the radar, effectively solving the long-standing problem of "can see but cannot pass" of the unmanned sweeper in narrow lanes and half-occluded scenes from the engineering practice point of view. This effect is not necessarily brought about by the existing layered obstacle avoidance technology.

[0023] In addition, since the layered V-Graph has reasonably expressed the feasible regions at different heights during the construction stage, the present application can more stably guide the search process to expand along the safe and passable path region when working with search algorithms such as hybrid A*, reducing the sensitive dependence on search weights and heuristic parameters, thereby improving the stability and engineering reproducibility of the path planning results. This effect is difficult to achieve in existing path planning schemes based on empirical parameter adjustment.

[0024] In summary, the present application combines the structural features of the sweeper, layered collision constraints and V-Graph generation mechanism organically, and achieves a comprehensive improvement in safety, passing ability and planning efficiency that is difficult to achieve in the prior art. BRIEF DESCRIPTION OF DRAWINGS

[0025] The present application will be further described below in conjunction with the drawings and examples, in which: Figure 1 A method step flow chart of a layered collision detection and path search method for a sweeper structure provided by the present application; Figure 2 A vehicle structure diagram of the upper wide and lower narrow structure for the layered collision detection and path search method provided by the present application; Figure 3 The Veno diagram of the present application compared with the unified layered collision detection for the layered collision detection and path search method provided by the present application; Figure 4 The Veno diagram of the present application compared with the non-unified layered collision detection for the layered collision detection and path search method provided by the present application; Figure 5 The step flow chart of the layered collision detection and path search method provided by the present application. DETAILED DESCRIPTION

[0026] In order to make the purpose, technical scheme and effect of the present application more clear and definite, the present application is further described in detail below. It should be understood that the specific embodiments described herein are only used to explain the present application, and are not used to limit the present application. The embodiments of the present application are introduced below in combination with the drawings.

[0027] The layered collision detection and path search method provided by the present application for the cleaning vehicle structure, please refer to Figures 1-5 , including the following steps: S1, obtaining point cloud data of the environment around the cleaning vehicle, and performing height layering on the point cloud data according to the distribution characteristics of the obstacles in the height direction, at least distinguishing into low-layer obstacles and high-layer obstacles; S2, based on the vehicle body structure of the cleaning vehicle with the upper wide and lower narrow structure, establishing a vehicle equivalent width model corresponding to different heights, and determining the width difference between the vehicle equivalent width corresponding to the high layer and the vehicle equivalent width corresponding to the low layer; S3, mapping the point cloud data after height layering to generate a grid map, and according to the width difference, performing inflation processing on the grid area corresponding to the high-layer obstacles, and performing inflation processing less than the high layer or no inflation processing on the grid area corresponding to the low-layer obstacles; S4, based on the grid map after completing the differential inflation processing, constructing a unified layered Veno diagram; S5, in the path search process, introducing the layered Veno diagram as path constraint or search guide information, planning the driving path of the cleaning vehicle, so as to improve the passing capacity in the complex and narrow scene under the premise of ensuring obstacle avoidance safety.

[0028] In the embodiment, first, point cloud data of the environment around the cleaning vehicle is acquired. The point cloud data can be acquired by a laser radar installed on the cleaning vehicle, and the sampling frequency and the number of points are set according to the operation requirements of the vehicle. The laser radar point cloud (10 Hz sampling, 10,000 points / frame) covers the main traffic areas in front of and to the side of the vehicle to reflect the distribution of obstacles in space, in order to adapt to unstructured scenes such as sidewalks, auxiliary roads, and village-in-city.

[0029] After obtaining the point cloud data, the point cloud data is processed by height layering according to the distribution characteristics of the obstacles in the height direction. Specifically, based on the density characteristics and height information of the point cloud in space, the point cloud is clustered and segmented, and the point cloud with spatial continuity is divided into several point cloud clusters; then, according to the height range of the points in each point cloud cluster, it is judged whether the corresponding obstacle is located below the blind filling radar of the cleaning vehicle or within the action height range of the blind filling radar, so as to at least distinguish low-layer obstacles and high-layer obstacles. Through the height layering processing, different height obstacles can be treated differently in the subsequent modeling process.

[0030] After completing the height layering of the obstacles, a vehicle equivalent width model corresponding to different heights is established based on the vehicle body structure of the cleaning vehicle which is wide at the top and narrow at the bottom. Specifically, the cleaning vehicle has a first vehicle body width when the blind filling radar is not considered, and in the area above the installation height of the blind filling radar, the overall outline width of the vehicle increases due to the outward extension of the blind filling radar, forming a second vehicle body width. According to the overall outline dimensions of the vehicle body at different heights, the width difference between the vehicle equivalent width corresponding to the high layer and the vehicle equivalent width corresponding to the low layer is determined, and the width difference is used to reflect the true traffic constraints of the cleaning vehicle at different heights in space.

[0031] Subsequently, the point cloud data processed by height layering is mapped to generate a grid map. The resolution of the grid map can be set according to the size of the cleaning vehicle and the complexity of the environment to ensure the expression accuracy of the obstacle outline. After generating the grid map, different height obstacles corresponding to the grid regions are subjected to differential inflation processing according to the width difference determined as described above. Specifically, for the grid region corresponding to the high-layer obstacle, inflation processing is performed according to the inflation scale determined by the width difference to ensure the safety obstacle avoidance requirement within the height range corresponding to the blind filling radar; and for the grid region corresponding to the low-layer obstacle, no inflation processing is performed, or inflation processing is performed with an inflation scale smaller than that of the high-layer obstacle, so as to reserve the effective traffic space below the blind filling radar.

[0032] After the differential inflation processing is completed, a unified hierarchical Voronoi diagram is constructed based on the grid map. The generated hierarchical Voronoi diagram simultaneously reflects the spatial relationship of high-level and low-level obstacles after being processed by different inflation strategies in structure, so that the edges and nodes in the Voronoi diagram can reflect the safe passing area at different heights. In this way, the hierarchical information no longer stays at the perception or grid level, but directly participates in the construction of the path planning basic structure.

[0033] In the path search process, the above-mentioned hierarchical Voronoi diagram is introduced as path constraint or search guide information to plan the driving path of the cleaning vehicle. Specifically, in the search process, the path is expanded along the safe passage indicated by the hierarchical Voronoi diagram in priority, and a feasible driving path is generated under the premise of meeting the kinematic constraints of the vehicle. By using the different height passing information reflected in the hierarchical Voronoi diagram, the cleaning vehicle can reasonably use the low-level space to pass through narrow areas while ensuring the safety of the blind radar obstacle avoidance, thereby improving the passing ability in complex narrow scenes.

[0034] Through the above-mentioned embodiments, the present application realizes reasonable modeling of passing constraints of obstacles at different heights without changing the hardware structure of the cleaning vehicle, so that the path planning process can take into account both safety and passing efficiency, and has good engineering applicability.

[0035] In further embodiments, the height hierarchy includes: Based on the spatial density and height information of the point cloud, the point cloud is clustered and segmented, and the height range of each point cloud cluster in the clustering result is used to determine the obstacle level to which it belongs.

[0036] In actual application, the running environment of the cleaning vehicle usually contains various obstacles such as road edges, walls, guardrails, tree trunks, and parked vehicles, which have different height distribution characteristics in space. To avoid noise interference caused by single-point height judgment, the present embodiment does not directly divide the height threshold of a single point, but first performs spatial clustering processing on the point cloud.

[0037] Specifically, based on the distribution density characteristics of the point cloud in three-dimensional space, the point cloud data is clustered and segmented, and the point clouds with adjacent spatial positions and continuous shapes are merged into several point cloud clusters. Each point cloud cluster is used to represent a relatively complete environmental entity or local structure of an obstacle. Through clustering processing, the influence of discrete noise points on height judgment can be effectively suppressed, and the stability of obstacle level determination can be improved.

[0038] After the point cloud clustering is completed, a height feature parameter of each point cloud cluster is extracted, such as the maximum height, the minimum height or the height distribution range of each point in the point cloud cluster. According to the height feature parameter, the obstacle corresponding to the point cloud cluster is divided into different height levels. When the height range of the point cloud cluster is mainly located in the space below the blind-filling radar of the sweeper, it is determined as a low-level obstacle; when the height range of the point cloud cluster involves or exceeds the action height of the blind-filling radar, it is determined as a high-level obstacle, and preferably, the height layering threshold is 0.5 m (lower than 0.5 m is a low-level obstacle, higher than 0.5 m is a high-level).

[0039] Through the above height layering method based on the clustering result, the level division of the obstacle is established on the basis of the spatial continuous structure, rather than the isolated point judgment, thereby providing reliable input data for subsequent differentiated inflation and hierarchical Voronoi diagram construction.

[0040] In further embodiments, the clustering segmentation is implemented by using a DBSCAN algorithm.

[0041] In the present embodiment, the point cloud clustering segmentation is implemented by using a DBSCAN algorithm based on density. The DBSCAN algorithm merges points with relatively high density and mutual connectivity into the same cluster by analyzing the local density distribution of the point cloud in space, and regards the discrete points with insufficient density as noise points, thereby being suitable for the point cloud segmentation task in a complex environment.

[0042] In specific implementation, the parameters of the DBSCAN algorithm are set according to the point cloud resolution of the sensor carried by the sweeper and the complexity of the environment. Among them, the neighborhood radius parameter is used to limit the spatial adjacency range between point clouds, and the minimum point number parameter is used to limit the minimum point number threshold required to form an effective point cloud cluster. By reasonably setting the above parameters, the DBSCAN algorithm can effectively filter out noise points caused by factors such as ground reflection and long-distance interference while retaining the overall structural features of the obstacle.

[0043] After the DBSCAN clustering is completed, each formed point cloud cluster is used as a basic unit for subsequent height analysis and obstacle level judgment. Compared with the segmentation method based on a regular grid or a simple height threshold, the DBSCAN algorithm can more accurately reflect the real spatial form of the obstacle, and is especially suitable for obstacles with irregular form and fuzzy boundary in an environment such as a sidewalk or a narrow alley.

[0044] By introducing the DBSCAN clustering mechanism in the point cloud height layering process, the obstacle level division process is more robust, and a reliable foundation is laid for subsequent collision detection modeling based on the structural difference of the vehicle.

[0045] In further embodiments, the vehicle equivalent width model is established based on the installation height of the blind-filling radar of the sweeper and the extension position thereof.

[0046] In the actual structure of the unmanned sweeper, in order to eliminate the sensing blind area around the vehicle, the blind-filling radar is usually installed on the upper part of the vehicle body and extends outward relative to the main body of the vehicle body. Thus, the sweeper presents a significantly different overall size in the height direction: in the height range below the blind-filling radar, the overall width of the vehicle is mainly determined by the main body of the vehicle body; and in the area above the installation height of the blind-filling radar and above, the overall width of the vehicle is determined by the main body of the vehicle body and the outwardly extending structure of the blind-filling radar.

[0047] Based on the above structural features, the embodiment does not use a single vehicle width parameter in the path planning process, but establishes a vehicle equivalent width model corresponding to different heights according to the installation height of the blind-filling radar and the spatial position relationship. Specifically, the height direction of the vehicle is divided into at least two intervals: a low-height interval below the blind-filling radar, and a high-height interval including the blind-filling radar structure; and corresponding vehicle equivalent width parameters are set for each height interval to reflect the true passing profile of the vehicle in that height range.

[0048] Specifically, a height-width mapping relationship is established, for example: The vehicle width at the height of the blind-filling radar is w1; The vehicle width below the blind-filling radar is w2; The vehicle width difference Aw = w1-w2 is calculated as the basis for adjusting the inflation radius.

[0049] In this way, the space occupation of the vehicle at different heights can be accurately described, providing structured input for subsequent height-layered collision detection and path planning, so that the structural features of the vehicle can directly participate in the planning modeling process.

[0050] In a further embodiment, the width difference is the difference between the width of the vehicle body including the blind-filling radar and the width of the vehicle body not including the blind-filling radar.

[0051] Specifically, first, the width of the vehicle body without the blind-filling radar structure is obtained, which is used to represent the basic passing width of the vehicle in the low-height interval; at the same time, the overall profile width of the vehicle including the blind-filling radar structure is obtained, which is used to represent the maximum passing width of the vehicle in the high-height interval. The above two widths can be obtained by pre-calibration of vehicle structure parameters, or calculated according to the installation position and extension size of the blind-filling radar.

[0052] Subsequently, according to the difference between the high-layer vehicle equivalent width and the low-layer vehicle equivalent width, a width difference parameter is determined for distinguishing the height passing constraints. The width difference directly reflects the additional space occupation introduced by the blind-filling radar structure relative to the main body of the vehicle body.

[0053] In the subsequent obstacle modeling process, the width difference is used as the basis for differential inflation processing, so that the safety avoidance distance corresponding to the high-level obstacle can fully cover the blind-filling radar structure, while the low-level obstacle does not have to bear the same degree of spatial constraint. By quantifying the vehicle structure difference as a clear width difference parameter, the vehicle structure feature can participate in the collision detection and path planning process in a calculable and reusable manner, avoiding relying on empirical parameters or manual adjustment.

[0054] In a further embodiment, the grid area corresponding to the high-level obstacle is inflated using an inflation radius determined based on the width difference.

[0055] After completing the height layering of the environment point cloud and the modeling of the equivalent width corresponding to the different heights of the vehicle, the layered point cloud data is mapped to generate a grid map with a resolution of 0.1 m x 0.1 m. The grid map is used to describe the occupancy of obstacles in the environment in the plane space and serves as the basis for subsequent path planning and collision detection.

[0056] For obstacles located in the high-level height range, the overall width of the sweeper in this height range includes the blind-filling radar structure, so the safety requirement for the surrounding space is significantly higher than that in the low-level height range. Based on the width difference parameter determined in the foregoing, the width difference is used as an important basis for inflation scale, and inflation processing is performed on the grid area corresponding to the high-level obstacle.

[0057] In specific implementation, the occupied area of the high-level obstacle in the grid map is taken as the center, and the inflation radius determined based on the width difference is r2=r1+Δw. The surrounding grid cells are expanded and marked, so that the high-level obstacle forms a larger occupied area in the grid map. Through the inflation processing, the safety avoidance distance in the height range of the blind-filling radar is fully reflected at the grid level, so that the risk of collision or rubbing between the sweeper and the high-level obstacle is effectively avoided in the path planning process.

[0058] Through the above high-level obstacle inflation strategy based on the vehicle structure difference, the obstacle modeling is no longer dependent on uniform and fixed safety distance parameters, but is directly related to the real structure features of the sweeper, enhancing the rationality and pertinence of the collision detection model.

[0059] In a further embodiment, the grid area corresponding to the low-level obstacle is not inflated, or is inflated with an inflation radius smaller than that of the high-level obstacle, to allow the sweeper to pass through the space below the blind-filling radar.

[0060] For low-layer obstacles located in the height interval below the blind-filling radar, the present embodiment does not adopt the same inflation strategy as high-layer obstacles. On the one hand, the overall outer width of the sweeper in the low-layer height interval is small, and the actual passing boundary of the vehicle in this height range is mainly determined by the vehicle body; on the other hand, if the same inflation processing as the high layer is applied to the low-layer obstacles, it will inevitably compress the effective space that can be passed below the blind-filling radar.

[0061] Based on the above considerations, the present embodiment does not perform inflation processing on the grid area corresponding to the low-layer obstacle, or performs inflation processing with an inflation radius significantly smaller than that of the high-layer obstacle, i.e., the inflation radius r1 is zero. This allows the low-layer space to be preserved under the premise of meeting the basic safety requirements, for the vehicle to pass through narrow passages or semi-occluded areas.

[0062] By implementing differentiated grid inflation processing on high-layer and low-layer obstacles, the grid map simultaneously expresses the passing constraint conditions at different heights in the same plane, thereby providing an accurate and distinguishable spatial foundation for the subsequent construction of the layered Voronoi diagram. This differentiated inflation strategy allows the sweeper to reasonably utilize the low-layer space to pass through complex narrow scenes while ensuring the safety of the blind-filling radar, effectively improving the actual passing capacity of the vehicle.

[0063] In further embodiments, the layered Voronoi diagram is generated by performing Delaunay triangulation on the grid map that has undergone differentiated inflation processing.

[0064] After completing the differentiated grid inflation processing of high-layer and low-layer obstacles, the resulting grid map simultaneously reflects the passing constraint conditions at different heights in the same plane. Based on this grid map, the present embodiment further constructs a unified layered Voronoi diagram for describing the potential safe passing channels of the sweeper in complex environments.

[0065] In specific implementation, first, the boundary information of the obstacle-occupied area in the grid map is extracted and used as the input constraint for generating the Voronoi diagram. Then, Delaunay triangulation operation is performed based on the boundary information to obtain the corresponding Voronoi structure. The Voronoi diagram generated in this way has its edges located at equidistant positions of adjacent obstacle boundaries, and its spatial position naturally reflects the safe passing channels between obstacles.

[0066] Since the grid map has undergone differentiated inflation processing on obstacles at different heights, the generated Voronoi diagram is not a single safety boundary structure in the traditional sense, but a layered Voronoi diagram structure that implicitly integrates different height passing constraints. This layered Voronoi diagram maintains unity in overall structure, but its edge distribution and node position already reflect the differences between high-layer safety requirements and low-layer passable space, providing a spatial skeleton with high differentiation ability for subsequent path search.

[0067] In further embodiments, the path search employs a hybrid A* algorithm, and the hierarchical Voronoi diagram is incorporated in the search process for node expansion or path evaluation.

[0068] In the present embodiment, the path search of the cleaning vehicle employs a hybrid A* algorithm. The hybrid A* algorithm considers both the kinematic constraints of the vehicle and the discrete search space in the search process, and is suitable for mobile platforms such as cleaning vehicles that have minimum turning radius and nonholonomic constraints.

[0069] The edge distance of the hierarchical Voronoi diagram is introduced in the heuristic function of the hybrid A* algorithm: F(x) = k_g*g(x) + k_h*h(x) + k_v*V(x) wherein: g(x) is a cumulative cost function; h(x) is a heuristic cost function; V(x) is the minimum distance between the current node and the Voronoi edge; k_g, k_h, k_v are different weight coefficients, respectively; The Reeds-Shepp curve is used to fit the path in the terminal region, to ensure that the path meets the kinematic constraints of the vehicle.

[0070] In the search process of the hybrid A* algorithm, the hierarchical Voronoi diagram generated as described above is introduced as search guide information. Specifically, in the node expansion phase, by judging the spatial relationship between the candidate node and the edge line of the hierarchical Voronoi diagram, the search process is guided to preferentially expand along the potential safe passage indicated by the Voronoi diagram, thereby reducing blind search in obstacle-dense areas.

[0071] Since the hierarchical Voronoi diagram has comprehensively reflected the safety avoidance requirements of high-level obstacles and the passable characteristics of low-level space, the search process under the guidance of the Voronoi diagram can naturally tend to the path region that meets both the safety requirements of the blind radar and has a large passage margin. Compared with the search method without introducing the Voronoi diagram guidance, this search strategy can significantly reduce the number of invalid node expansions in complex and narrow environments, and improve the search efficiency and the stability of the path quality.

[0072] In further embodiments, the distance from the path node to the edge line of the hierarchical Voronoi diagram is introduced as an evaluation parameter in the cost function or heuristic function of the hybrid A* algorithm.

[0073] In the present embodiment, the distance from the path node to the edge line of the hierarchical Voronoi diagram is introduced as one of the evaluation parameters in the cost function or heuristic function of the hybrid A* algorithm. This distance is used to measure the degree of deviation of the current node from the potential safe passage. When the node is close to the edge line of the Voronoi diagram, it indicates that the node is in a relatively safe and large passage margin region; otherwise, it indicates that the node may be close to the obstacle boundary.

[0074] By incorporating the aforementioned distance parameters into the cost function or heuristic function, the search process can reflect the differences in safety and accessibility among different nodes during the cost evaluation stage, guiding the search results to extend more smoothly and stably along the safe path reflected by the hierarchical Venn diagram. This approach avoids the instability caused by relying solely on geometric distance or empirical weights for path evaluation, reduces the sensitivity of path planning results to parameter adjustments, and better meets actual engineering needs.

[0075] Through the above implementation methods, the present invention realizes a complete technical closed loop from obstacle layer modeling and differentiated spatial expression to layered Venn diagram construction and search collaborative guidance, enabling the sweeper to have higher safety, traffic efficiency and stability in the path planning process in complex and narrow scenarios.

[0076] In summary, this invention, through in-depth analysis of the actual structural characteristics of the sweeper vehicle, breaks through the traditional path planning modeling approach that treats the vehicle as a uniform shape in the height direction. It introduces the vehicle's passage constraints at different heights into the entire collision detection and path planning process in a calculable and reusable manner. By performing height-level layering of the environmental point cloud and establishing equivalent vehicle width models corresponding to different heights based on the sweeper's top-wide, bottom-narrow structural characteristics, obstacle modeling can realistically reflect the vehicle's safety avoidance requirements at different heights.

[0077] Building upon this foundation, the present invention implements differentiated grid expansion processing for obstacles of varying heights and constructs a unified hierarchical Venograph accordingly. This enables the path planning infrastructure itself to distinguish between different levels of passage space. By introducing the hierarchical Venograph as constraint or guidance information during path search, the sweeper can meet the obstacle avoidance requirements of blind spot radar while efficiently utilizing lower-level effective passage space to navigate complex and narrow scenarios, thus achieving a good balance between safety and passage efficiency.

[0078] Compared to existing technologies that only introduce layered collision detection at the perception layer or post-processing stage, this invention integrates layered information upfront and deeply into the path planning process, thereby altering the search behavior itself and avoiding the shortcomings of traditional methods that rely on repeated path corrections or adjustments based on empirical parameters. The overall solution has a clear structure and well-defined implementation path, exhibiting good engineering feasibility and stability. It is suitable for autonomous driving devices with irregular vehicle structures, such as unmanned cleaning vehicles, and has significant application value in complex environments.

[0079] It should be understood that the application of the present invention is not limited to the examples above. Those skilled in the art can make improvements or modifications based on the above description, and all such improvements and modifications should fall within the protection scope of the appended claims.

Claims

1. A layered collision detection and path search method for a sweeper vehicle structure, characterized in that, Includes the following steps: Acquire point cloud data of the environment surrounding the sweeper, and perform height layering of the point cloud data based on the distribution characteristics of obstacles in the height direction, at least distinguishing them into low-level obstacles and high-level obstacles; Based on the sweeper's body structure, which is wider at the top and narrower at the bottom, an equivalent width model of the vehicle corresponding to different heights is established to determine the width difference between the equivalent width of the vehicle corresponding to higher heights and the equivalent width of the vehicle corresponding to lower heights. The point cloud data after height layering is mapped to generate a raster map. Based on the width difference, the raster area corresponding to the high-level obstacle is expanded, and the raster area corresponding to the low-level obstacle is expanded by a smaller amount than the high-level obstacle or not expanded. Based on the raster map that has undergone differential expansion processing, a unified hierarchical Vinio map is constructed. During the path search process, the hierarchical Veno diagram is introduced as a path constraint or search guidance information to plan the driving path of the sweeper, so as to improve the passage ability in complex and narrow scenarios while ensuring obstacle avoidance safety.

2. The hierarchical collision detection and path search method according to claim 1, characterized in that, The height layering includes: Based on the spatial density and height information of point clouds, the point clouds are clustered and segmented, and the obstacle level to which each point cloud cluster belongs is determined according to the height range of each point cloud cluster in the clustering results.

3. The hierarchical collision detection and path search method according to claim 2, characterized in that, The clustering and segmentation are implemented using the DBSCAN algorithm.

4. The hierarchical collision detection and path search method according to claim 1, characterized in that, The vehicle equivalent width model is established based on the installation height and extension position of the sweeper's blind spot radar.

5. The hierarchical collision detection and path search method according to claim 4, characterized in that, The width difference is the difference between the width of the sweeper vehicle with and without the blind spot radar.

6. The hierarchical collision detection and path search method according to claim 1, characterized in that, The grid area corresponding to the high-rise obstacle is expanded using an expansion radius determined based on the width difference.

7. The hierarchical collision detection and path search method according to claim 1, characterized in that, The grid area corresponding to the lower-level obstacle is not expanded, or is expanded with a radius smaller than that of the higher-level obstacle, so as to allow the sweeper to pass through the space below the blind spot radar.

8. The hierarchical collision detection and path search method according to claim 1, characterized in that, The hierarchical Vinograph is generated by performing Delaunay triangulation on a raster map that has undergone differential dilation processing.

9. The hierarchical collision detection and path search method according to claim 1, characterized in that, The path search employs a hybrid A* algorithm, and during the search process, it incorporates the hierarchical Venograph for node expansion or path evaluation.

10. The hierarchical collision detection and path search method according to claim 9, characterized in that, In the cost function or heuristic function of the hybrid A* algorithm, the distance from the path node to the edge of the hierarchical Vinograph is introduced as an evaluation parameter.

Images