Filtering method for abnormal planning path of unmanned sweeper

Abstract

The application discloses a filtering method for an abnormal planning path of an unmanned sweeper, and the method comprises the following steps: dividing a space around a vehicle into a plurality of regions; obtaining and counting obstacle information in each region and evaluating the obstacle information to obtain a high-risk obstacle region; taking a current vehicle position and a target point as key waypoints based on a vehicle planning path, selecting two other key waypoints between the current vehicle position and the target point through a waypoint sampling method, and calculating a circle center fitting point of the four key waypoints; judging whether the fitting circle center is located in the effective region behind the rear axle of the vehicle, and if yes, the current planning is invalid; starting a re-planning process for the invalid current planning; recalculating a path according to new waypoint information, and updating a motion plan of the vehicle. The application introduces a trajectory evaluation mechanism to perform secondary auditing on the path, so as to ensure the driving safety of the unmanned grassland vehicle.

Description

Technical Field

[0001] This invention belongs to the field of autonomous driving technology, specifically relating to a method for filtering abnormal planned paths of unmanned sweeping vehicles. Background Technology

[0002] In unmanned sweeping vehicle systems, path planning is a crucial step in ensuring the vehicle's safe and efficient operation. The planner typically consists of two parts: a module based on classical algorithms responsible for generating the basic path; and a deep learning model trained on extensive data to optimize path selection. This planner, combining the stability of traditional algorithms with the flexibility of deep learning, can adapt to changing environmental conditions and provide the unmanned sweeping vehicle with a relatively optimal driving path.

[0003] However, unmanned sweeping vehicles often need to operate in open and complex road environments, including but not limited to city streets, park paths, and commercial areas. The complexity of these scenarios is mainly reflected in the frequent appearance of static obstacles (such as buildings and trees) and dynamic obstacles (such as pedestrians and other vehicles), as well as the impact of unpredictable factors such as changes in lighting and weather conditions on the system's positioning accuracy. Due to these factors, even advanced planners may face challenges in handling these situations. In these extreme cases, the planner may output a theoretically feasible path that is actually potentially dangerous. For example, the path may be too close to moving obstacles, or under certain conditions (such as slippery surfaces) it may cause the vehicle to lose control. Therefore, to ensure the safety of unmanned sweeping vehicles, additional measures must be taken to filter out these potentially risky trajectories. Summary of the Invention

[0004] To address the shortcomings of existing technologies and achieve secondary review of planned routes, this invention adopts the following technical solution:

[0005] A method for filtering abnormal planned paths of unmanned sweeping vehicles includes the following steps:

[0006] Step S1: Divide the space around the vehicle into regions to obtain a set of regions;

[0007] Step S2: Obtain and analyze obstacle information in each area and evaluate it to identify high-risk obstacle areas;

[0008] Step S3: Based on the vehicle's planned path, the current vehicle position and the target point are taken as key waypoints. Then, using the waypoint sampling method, two other key waypoints are selected between the current vehicle position and the target point, and the center fitting points of the four key waypoints are calculated.

[0009] Step S4: Determine whether the position of the fitted circle center is located within the effective area behind the rear axle of the vehicle. If so, the current plan is invalid to prevent the vehicle from driving on an unsafe path.

[0010] Step S5: For invalid current plans, initiate the replanning process;

[0011] Step S6: Recalculate the route based on the new waypoint information and update the vehicle's motion plan to ensure real-time response in dynamic environments.

[0012] Furthermore, in step S1, the space surrounding the vehicle is divided into four regions, with the rear axle as the dividing line. Each region corresponds to different environmental characteristics and constraints, providing a basis for subsequent path planning. The four regions are:

[0013] The area on the left front of the vehicle is used to assess obstacles and driving conditions on the left front of the vehicle to ensure safe driving on the left side;

[0014] The area on the right front of the vehicle is used to assess obstacles on the right front of the vehicle to ensure safety when driving on the right and keeping close to the edge.

[0015] The area on the left rear of the vehicle is used to monitor obstacles on the left rear of the vehicle in order to avoid collisions.

[0016] The area on the right rear of the vehicle is used to monitor obstacles on the right rear of the vehicle to ensure safety when turning and keeping to the side.

[0017] Furthermore, in step S2, based on the type and area of ​​the obstacle, the weighted area of ​​the obstacle is calculated, the weighted area of ​​all obstacles in the effective area is statistically analyzed and its proportion relative to the effective area is calculated to obtain the obstacle ratio, and the area with the obstacle ratio higher than the threshold is regarded as the high-risk obstacle area.

[0018] If a high-risk obstacle area exists, the vehicle is divided into subdivided areas from near to far. The proportion of obstacles in each subdivided area is calculated sequentially until the proportion of obstacles in each subdivided area is higher than the subdivision threshold. The calculated areas that are lower than the subdivision threshold are then considered safe areas.

[0019] Further, step S2 includes the following steps:

[0020] Step S2.1: Acquire obstacle data in each region (FL, FR, RL, RR) in real time, including the number, type, size and location of obstacles in the environment;

[0021] Step S2.2: For each region X, calculate the total area A of the obstacles. X They are recorded as follows:

[0022] ;

[0023] Step S2.3: Set the area of ​​each region and set the effective detection area as a sector area. :

[0024] in, The detection radius for each region, The central angle of each sector (expressed in radians);

[0025] Step S2.4: Obstacle ratio calculation, considering the impact of different obstacle types on safety, and setting weight coefficients for different obstacle types. This yields the proportion of each region:

[0026]

[0027] Where i represents the i-th obstacle in region X, and ∑ represents the summation of the products of the areas of all obstacles in region X and their corresponding weight coefficients;

[0028] Step S2.5: Set an anomaly threshold for each region. If the proportion of a certain region exceeds the threshold... This indicates the presence of a high-risk obstacle area.

[0029] Furthermore, the sector area in step S2.3:

[0030]

[0031] Where N represents the number of regions. This represents the detection radius of region X. This represents the central angle of the sector corresponding to region X.

[0032] Furthermore, the key waypoint extraction and circle center fitting process in step S3 includes the following steps:

[0033] Step S3.1: Path planning acquisition. Extract the planned path of the current vehicle to obtain a series of continuous waypoints:

[0034]

[0035] Where n represents the number of waypoints;

[0036] Step S3.2: Use the target point and the vehicle's current position as fixed sampling points to determine two key waypoints:

[0037] ;

[0038] Step S3.3: Using the waypoint sampling method, select two more waypoints from the extracted path waypoints to form a complete set of four key waypoints with the fixed sampling point. The extraction process for the other two waypoints is as follows:

[0039] Calculate the direction vector from the current waypoint to the target point:

[0040] ;

[0041] Calculate the unit vector of this direction vector:

[0042]

[0043] in, express The model;

[0044] Between the current waypoint and the target point, traverse the path waypoints and select two path waypoints in the direction of the unit vector U;

[0045] Step S3.4: Obtain the coordinates of the four key waypoints and record them as follows:

[0046]

[0047]

[0048]

[0049] ;

[0050] Step S3.5: Calculate the coordinates of the fitted circle center based on the coordinate information of the four key waypoints. ;

[0051] The formula for calculating the center of the fitted circle is:

[0052]

[0053] .

[0054] Further, in step S3.3, for two equidistant waypoints, the curvature of each waypoint is calculated:

[0055] ,

[0056] Among them, (x i ,y i (x) represents the current waypoint coordinates. i+1 ,y i+1 (x) represents the coordinates of the selected waypoints. i+2 ,y i+2 () represents the coordinates of the target point;

[0057] Choose the waypoint with the greatest curvature. Second largest waypoint At the same time, ensure that the distance d between them satisfies a certain minimum threshold T;

[0058] Calculate the distance between two waypoints:

[0059]

[0060] If d < T, then reselect the waypoint with the second largest curvature until d ≥ T is satisfied; and This refers to the sampling waypoints between the target point and the vehicle's current location.

[0061] Furthermore, the effective region in step S4 is a fan-shaped region, and the determination condition is as follows:

[0062] Angle conditions: Within [-θ / 2, θ / 2];

[0063] Distance conditions:

[0064] Among them, (x c ,y c (x) represents the coordinates of the fitted circle center. rear ,y rear ) represents the rear axis position coordinates, θ represents the center angle of the sector, and R represents the detection radius of the sector area.

[0065] Furthermore, in step S4, fan-shaped regions are set for the rear left and rear right regions of the rear axle respectively, and the judgment condition is:

[0066] For the rear left region RL, the judgment condition is:

[0067] Angle conditions: exist Inside

[0068] Distance conditions:

[0069] For the right region RR, the judgment condition is:

[0070] Angle conditions: exist Inside

[0071] Distance conditions:

[0072] in, These represent the detection radii of the rear left and rear right regions, respectively. These represent the central angles of the rear left and rear right regions, respectively.

[0073] Furthermore, in step S4, for invalid current plans, an invalid mark is made, and the number of invalidities (count_invalid) is recorded, generating a warning message; the replanning process is started, and a message is displayed indicating that the current path is unsafe and suggesting that an alternative path be selected;

[0074] In step S5, if replanning is still unsuccessful, the target waypoint is dynamically adjusted to ensure that the waypoint is reasonable and non-high-risk obstacle areas (areas with fewer obstacles) are selected as priority planning areas; if the planned trajectory chooses to pass through areas with more obstacles, a prompt is issued, suggesting that the path be adjusted to avoid high obstacle areas.

[0075] The advantages and beneficial effects of this invention are as follows:

[0076] The present invention provides a method for filtering abnormal planned paths of unmanned sweeping vehicles. It introduces a trajectory evaluation mechanism to conduct a secondary review after the planner outputs the path. If a path is detected as not meeting safety standards, the system needs to be able to reject the path and require the planner to recalculate, thereby filtering out the risky trajectories planned by the moving unmanned sweeping vehicle and ensuring the driving safety of the unmanned sweeping vehicle. Attached Figure Description

[0077] Figure 1 This is a flowchart of the method in an embodiment of the present invention. Detailed Implementation

[0078] The specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are for illustration and explanation only and are not intended to limit the present invention.

[0079] On city streets, an unmanned sweeper needs to perform daily cleaning operations. Due to the complex street environment and the presence of various static and dynamic obstacles, the unmanned sweeper's abnormal path planning filtering method of this invention is needed to ensure safe and efficient operation.

[0080] like Figure 1 As shown, a method for filtering abnormal planned paths of an unmanned sweeping vehicle is provided. The unmanned sweeping vehicle is equipped with at least a lidar, a wheeled odometer, and an inertial measurement unit, and includes the following steps:

[0081] Step S1: Divide the space around the vehicle into four areas, with the rear wheel axle as the dividing line. Each area corresponds to different environmental characteristics and constraints, providing a basis for subsequent path planning.

[0082] To effectively plan routes, the environment surrounding the vehicle is divided into four specific zones, with the rear axle as the dividing line:

[0083] FL area: This area is mainly used to assess obstacles and driving conditions on the left front of the vehicle to ensure driving safety on the left; FR area: This area focuses on assessing obstacles on the right front of the vehicle to ensure safety when driving on the right and keeping to the side; RL area: This area is responsible for monitoring obstacles on the left rear of the vehicle to avoid collisions; RR area: This area monitors obstacles on the right rear of the vehicle to ensure safety when turning and keeping to the side.

[0084] Step S2: Based on sensor data, count the number of obstacles in each area, calculate the proportion, and score each area. Areas with fewer obstacles receive higher scores. The obstacle calculation includes the following steps:

[0085] Step S2.1: Data collection. Use the vehicle's sensor system (such as lidar, camera, ultrasonic sensor) to collect obstacle data in real time in four areas (FL, FR, RL, RR), including the number, type, size and location of obstacles in the environment.

[0086] Step S2.2: For each region, calculate the total area of ​​the obstacles and record it as follows:

[0087]

[0088] Step S2.3: Set the area of ​​each region. Set the effective detection area as a sector area for each region and record it as follows:

[0089]

[0090] in, The detection radius for each region, The central angle of each sector (expressed in radians);

[0091] Step S2.4: Obstacle ratio calculation, considering the impact of different obstacle types on safety, and setting weighting coefficients. This allows us to obtain the proportion of each region:

[0092]

[0093] Step S2.5: Anomaly Handling. If the total obstacle area or effective detection area data of a certain region is abnormal, set a threshold. :like This indicates the presence of a high-risk obstacle area.

[0094] Step S3: Extract key waypoints from the planned path, determine fixed sampling points including the target point and the vehicle's current position, select two additional waypoints according to the waypoint sampling method, obtaining a total of four circle center fitting points, and generating the circle center fitting formula; the key waypoint extraction and circle center fitting process includes the following steps:

[0095] Step S3.1: Path planning acquisition. Extract the planned path from the current vehicle's path planning algorithm to obtain a series of consecutive waypoints, and record them as follows:

[0096]

[0097] Where n represents the number of waypoints.

[0098] Step S3.2: Determine key waypoints, set the target point and the vehicle's current position as fixed sampling points, and record them as follows:

[0099]

[0100] Step S3.3: Use the waypoint sampling method to select two more waypoints from the extracted waypoints to form a complete set of four key waypoints; the specific logic is as follows:

[0101] Calculate the direction vector from the current position to the target point:

[0102]

[0103] Calculate the unit vector of this direction vector:

[0104]

[0105] in, express The model;

[0106] Between the current waypoint and the target point, traverse the waypoints along the path and select two waypoints that are equidistant in the U direction. and Prioritize locations with greater curvature:

[0107] Calculate the curvature of each waypoint:

[0108]

[0109] Next, choose the waypoint with the greatest curvature. Second largest waypoint At the same time, ensure that the distance d between them satisfies a certain minimum threshold T;

[0110] Finally, calculate the distance between the two waypoints:

[0111]

[0112] If d < T, then reselect the waypoint with the second largest curvature until d ≥ T is satisfied; and This refers to the sampling waypoints between the target point and the vehicle's current location;

[0113] Step S3.4: Coordinate extraction, obtain the coordinate information of four key waypoints and record them as follows:

[0114]

[0115]

[0116]

[0117]

[0118] Step S3.5: Calculate the center of the circle, based on the four waypoints, to determine the coordinates of the fitted center of the circle. The formula for calculating the center of the fitted circle is:

[0119]

[0120] .

[0121] Step S4: Calculate the position of the fitted circle center and check if it is located in the area behind the rear axle. If the fitted circle center is located in this area, the current plan is considered invalid to prevent the vehicle from traveling on an unsafe path. The fitted circle center position check and validity judgment process includes the following steps:

[0122] Step S4.1: Check whether the center of the fitted circle is located in the fan-shaped area behind the rear axle. The judgment condition is: set the position of the vehicle's rear axle as... Define the area behind the rear axle as a sector-shaped region, with the following parameters: : These are the detection radii for the rear left and rear right regions, respectively; : These are the center angles of the rear left and rear right regions, respectively (in radians).

[0123] For the rear left region (RL), the condition is:

[0124] Angle conditions: exist Inside

[0125] Distance conditions:

[0126] For the right rear region (RR), the judgment condition is:

[0127] Angle conditions: exist Inside

[0128] Distance conditions:

[0129] Step S4.2: Determine the validity of the current plan. If the center of the fitted circle is located in any sector region behind the rear axis, the current plan is considered invalid, and the following processing is performed:

[0130] Mark the status as invalid, record the number of invalid instances (count_invalid), and generate a warning message;

[0131] The system initiates a replanning process, prompting the user that the current path is unsafe and suggesting that they choose an alternative path.

[0132] Step S5: If the current plan is invalid, the system will initiate a replanning process. If replanning is still unsuccessful, the target waypoints will be dynamically adjusted to ensure they are reasonable. The area with the highest score will be selected as the priority planning area. If the planned trajectory passes through an area with many obstacles, the system will issue a prompt suggesting that the route be adjusted to avoid areas with high obstacles.

[0133] Step S6: Recalculate the route based on the new waypoint information and update the vehicle's motion plan to ensure real-time response in dynamic environments.

[0134] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for filtering abnormal planned paths of unmanned sweeping vehicles, characterized in that... Includes the following steps: Step S1: Divide the space around the vehicle into regions to obtain a set of regions; Step S2: Obtain and analyze obstacle information in each area and evaluate it to identify high-risk obstacle areas; Step S3: Based on the vehicle's planned path, the current vehicle position and the target point are taken as key waypoints. Then, using the waypoint sampling method, two other key waypoints are selected between the current vehicle position and the target point, and the center fitting points of the four key waypoints are calculated. Step S4: Determine whether the position of the fitted circle center is located within the effective area behind the vehicle's rear axle. If so, the current planning is invalid. The effective area is a fan-shaped region, and its determination condition is: Angle conditions: Within [-θ / 2, θ / 2]; Distance conditions: Among them, (x c ,y c (x) represents the coordinates of the fitted circle center. rear ,y rear ) represents the rear axis position coordinates, θ represents the center angle of the sector, and R represents the detection radius of the sector area; Step S5: For invalid current plans, initiate the replanning process; Step S6: Recalculate the route based on the new waypoint information and update the vehicle's movement plan.

2. The method for filtering abnormal planned paths of an unmanned sweeping vehicle according to claim 1, characterized in that: In step S1, the space around the vehicle is divided into four regions, with the rear wheel axle as the dividing line. Each region corresponds to different environmental characteristics and constraints. The four regions are: The area on the left front of the vehicle is used to assess obstacles and driving conditions on the left front of the vehicle. The area on the right front of the vehicle is used to assess obstacles on the right front of the vehicle. The area on the left rear of the vehicle is used to monitor obstacles on the left rear of the vehicle. The area to the right rear of the vehicle is used to monitor obstacles to the right rear of the vehicle.

3. The method for filtering abnormal planned paths of an unmanned sweeping vehicle according to claim 1, characterized in that: In step S2, based on the type and area of ​​the obstacle, the weighted area of ​​the obstacle is calculated, the weighted area of ​​all obstacles in the effective area is counted and its proportion relative to the effective area is calculated to obtain the obstacle ratio, and the area with the obstacle ratio higher than the threshold is regarded as the high-risk obstacle area.

4. The method for filtering abnormal planned paths of an unmanned sweeping vehicle according to claim 3, characterized in that: Step S2 includes the following steps: Step S2.1: Acquire obstacle data in each area in real time, including the number, type, size and location of obstacles; Step S2.2: For each region X, calculate the total area A of the obstacles. X ; Step S2.3: Set the area of ​​each region and set the effective detection area as a sector area. ; Step S2.4: Obstacle ratio calculation, setting weight coefficients for different types of obstacles. This yields the proportion of each region: Where i represents the i-th obstacle in region X, and ∑ represents the summation of the products of the areas of all obstacles in region X and their corresponding weight coefficients; Step S2.5: Set an anomaly threshold for each region. If the proportion of a certain region exceeds the threshold... This indicates the presence of a high-risk obstacle area.

5. The method for filtering abnormal planned paths of an unmanned sweeping vehicle according to claim 4, characterized in that: The sector area in step S2.3: Where N represents the number of regions. This represents the detection radius of region X. This represents the central angle of the sector corresponding to region X.

6. The method for filtering abnormal planned paths of an unmanned sweeping vehicle according to claim 1, characterized in that: The key waypoint extraction and circle center fitting process in step S3 includes the following steps: Step S3.1: Path planning acquisition, extract the planned path of the current vehicle to obtain a series of continuous waypoints; Step S3.2: Use the target point and the vehicle's current position as fixed sampling points to determine two key waypoints; Step S3.3: Using the waypoint sampling method, select two more key waypoints from the extracted path waypoints to form a complete set of four key waypoints with the fixed sampling point. The extraction process for the other two waypoints is as follows: Calculate the direction vector from the current waypoint to the target point; Calculate the unit vector of this direction vector; Between the current waypoint and the target point, traverse the path waypoints and select two path waypoints in the unit vector direction; Step S3.4: Obtain the coordinates of the four key waypoints; Step S3.5: Calculate the coordinates of the fitted circle center based on the coordinate information of the four key waypoints.

7. The method for filtering abnormal planned paths of an unmanned sweeping vehicle according to claim 6, characterized in that: In step S3.3, for two equidistant waypoints, the curvature of each waypoint is calculated: ， Among them, (x i ,y i (x) represents the current waypoint coordinates. i+1 ,y i+1 (x) represents the coordinates of the selected waypoints. i+2 ,y i+2 () represents the coordinates of the target point; Choose the waypoint with the greatest curvature. Second largest waypoint At the same time, ensure that the distance between them meets a certain minimum threshold.

8. The method for filtering abnormal planned paths of an unmanned sweeping vehicle according to claim 1, characterized in that: In step S4, fan-shaped regions are set for the rear left and rear right regions of the rear axle, respectively. The judgment condition is: For the rear left region RL, the judgment condition is: Angle conditions: exist Inside Distance conditions: For the right region RR, the judgment condition is: Angle conditions: exist Inside Distance conditions: in, These represent the detection radii of the rear left and rear right regions, respectively. These represent the central angles of the rear left and rear right regions, respectively.

9. The method for filtering abnormal planned paths of an unmanned sweeping vehicle according to claim 1, characterized in that: In step S4, invalid current plans are marked as invalid, the number of invalid instances is recorded, and a warning message is generated. The process of replanning is initiated, and a message is displayed indicating that the current path is not safe, suggesting that an alternative path be selected; In step S5, if replanning is still unsuccessful, the target waypoint is dynamically adjusted, and areas with fewer obstacles (i.e., areas with fewer obstacles) are selected as priority planning areas. If the planned trajectory passes through areas with more obstacles, a prompt is issued, suggesting that the path be adjusted to avoid areas with high obstacles.

Images