Sanitation vehicle welt driving path planning method and device

Abstract

The invention provides a sanitation vehicle welt driving path planning method and device, and the method comprises the following steps: obtaining a road boundary point, and dividing the road boundary point into a left boundary point and a right boundary point; determining whether the welt driving mode is a left welt or a right welt according to an upper-layer instruction; generating a plurality of target sampling points on the basis of road boundary points on the side indicated by the upper-layer instruction, wherein the target sampling points are located in a safe area after the road boundary points deviate towards the inner side of the vehicle by a preset safe distance in the normal direction of the road boundary points; taking the current position of the vehicle as a starting point, and connecting the starting point with the target sampling point to generate a path cluster; calculating the cost of each path in the path cluster according to a pre-designed cost function; and selecting the path with the minimum cost as the optimal path for the sanitation vehicle to run along the edge. The method does not need to depend on an accurate map and real-time positioning, and is suitable for a low-speed welting operation scene.

Description

Technical Field

[0001] The present invention belongs to the technical field of path planning, and in particular relates to a method and device for planning a path for a sanitation vehicle traveling close to the edge of a road. Background Art

[0002] During road sweeping operations, sanitation vehicles often need to drive close to the road's edge to ensure effective cleaning. Path planning for these vehicles is a key technology in the field of autonomous driving.

[0003] Existing technologies for vehicle-side driving route planning typically employ the following approach: First, an offline or original map of the road the vehicle is on is obtained. Using the vehicle's real-time positioning data, a primary target path or guidance path is planned based on the acquired map. Furthermore, based on the relative positional relationship between the primary target path and the actual road boundary, a secondary target path that adheres to the road boundary is calculated. This approach is essentially a two-stage route planning strategy that relies on accurate map information and real-time, precise positioning to achieve route planning that adheres to the road boundary.

[0004] However, the existing technical solutions have the following obvious deficiencies: On the one hand, such solutions rely heavily on maps and vehicle positioning data. If the vehicle positioning signal is lost, or if map information is missing or updates are delayed, the vehicle will be unable to properly plan a path that stays close to the edge of the road, seriously affecting the continuity and stability of the sanitation vehicle's cleaning operations.

[0005] On the other hand, even if the map and positioning information are complete, the specific implementation process of the above two-stage planning method is relatively complicated. It is necessary to calculate the first positioning path and the second positioning path separately, and then further calculate the first edge path and the second edge path, and then fuse these paths. This complex multi-step calculation process is not suitable for sanitation vehicles in low-speed operation scenarios. It increases the system computing power requirements and reduces the real-time response capability, making it difficult to meet the specific needs of actual sanitation vehicles' edge cleaning operations.

[0006] Therefore, existing edge-of-the-road path planning methods are not effectively applicable in scenarios where maps are unavailable or positioning signals are lost. Furthermore, due to the complexity of the planning process, they are unable to provide efficient and accurate path planning solutions for the low-speed operation characteristics of sanitation vehicles. To address these shortcomings, a path planning technology that does not rely on precise maps and real-time positioning and is suitable for low-speed, edge-of-the-road operation scenarios is urgently needed in the field. Summary of the Invention

[0007] The purpose of the present invention is to address the deficiencies in the above-mentioned background technology and to provide a method and device for planning the path of a sanitation vehicle driving close to the edge, which does not rely on accurate maps and real-time positioning and is suitable for low-speed, close-to-the-edge operation scenarios.

[0008] The technical solution adopted by the present invention is: a method for planning a path for a sanitation vehicle to drive close to the edge, comprising the following steps: Acquire road boundary points, and divide the road boundary points into left boundary points and right boundary points; Determine the side-to-side driving mode as left side or right side according to the upper layer instruction; Generate multiple target sampling points based on the road boundary point on the side indicated by the upper-level instruction, wherein the target sampling points are located in a safety area after the road boundary point is offset toward the vehicle inner side by a preset safety distance along its normal direction; Taking the current position of the vehicle as the starting point, connecting the starting point with the target sampling point to generate a path cluster; Calculating the cost of each path in the path cluster according to a pre-designed cost function; the cost function includes multiple cost terms, including at least an obstacle distance cost reflecting the distance between the vehicle and the selected road boundary, a heading deviation cost reflecting the angle between the path and the desired driving direction, a curvature cost reflecting the magnitude of the path curvature, and a smoothness cost reflecting the rate of change of the path curvature; The path with the minimum cost is selected as the optimal path for the sanitation vehicle to travel close to the side of the road.

[0009] In the above technical solution, a polynomial curve algorithm is used to connect the current position of the vehicle with each of the target sampling points to generate candidate driving paths that meet the requirements of curvature continuity and path smoothness as a path cluster.

[0010] In the above technical solution, the process of generating the target sampling points includes: The vehicle's current position is used as the origin of the vehicle coordinate system, the vehicle's forward direction is used as the longitudinal axis, and a preset foresight distance is set on the axis. The preset foresight distance is used as the longitudinal coordinate of all target sampling points. Obtaining a boundary point located at or near the longitudinal coordinate and belonging to the road boundary side specified by the upper-level instruction; The preset safety distance is offset inward of the vehicle's drivable area along the normal direction of the boundary point, and the lateral coordinate of the offset position is used as the lateral coordinate of the starting target sampling point; The horizontal coordinates of the remaining target sampling points are obtained by increasing or decreasing the horizontal coordinates of the initial target sampling point at preset horizontal intervals.

[0011] In the above technical solution, the calculation of the obstacle distance cost includes: Discretely sample the vehicle trajectory at a preset longitudinal sampling interval on each candidate path; At each sampling point, the minimum distance between the vehicle and the road boundary is calculated based on the vehicle outer contour and the set of valid road boundary points; Inputting the minimum distance into a monotonically decreasing penalty function to obtain a local distance penalty value, and summing the penalty values ​​of all sampling points to obtain the obstacle distance cost of the candidate path; When the minimum distance is greater than a preset safety threshold, the corresponding local penalty value is set to zero, so that the cost is not increased when the safety distance is met.

[0012] In the above technical solution, the heading deviation cost is obtained by calculating the angle between the tangential direction of the candidate path and the desired driving direction of the vehicle at each sampling point, and integrating or accumulating the absolute value or square of the angle along the entire path.

[0013] In the above technical solution, the curvature cost is obtained by discretely sampling the curvature value along the candidate path and accumulating or integrating the square of the curvature of each sampling point, so as to penalize the path with larger curvature.

[0014] In the above technical solution, the smoothness cost is obtained by calculating the difference in curvature values ​​between adjacent sampling points to obtain the curvature change rate, and accumulating or integrating the square of the curvature change rate over the entire candidate path to penalize paths with abrupt curvature changes, thereby ensuring driving smoothness.

[0015] In the above technical solution, when processing the road boundary points, adjacent road boundary points whose distances are less than a preset distance threshold are filtered out.

[0016] The present invention provides a path planning device for an automatic driving sanitation vehicle, which is used to implement the method described in the above technical solution, including: A boundary acquisition module, used to obtain road boundary point data in the vehicle's surrounding environment; A boundary processing module is used to pre-process the acquired boundary point data and divide it into a left boundary point set and a right boundary point set according to the distribution of the boundary points in the vehicle coordinate system; A target sampling module is configured to generate a plurality of target sampling points based on the road boundary point on the side indicated by the upper-layer instruction, wherein the target sampling points are located in a safety area after the road boundary point is offset toward the vehicle inner side by a preset safety distance along its normal direction; a path generation module, configured to connect the vehicle's current position with each of the target sampling points using a polynomial smooth curve algorithm to generate a plurality of candidate driving paths; a cost calculation module, configured to calculate the cost of each path in the path cluster according to a pre-designed cost function; the cost function comprising a plurality of cost items, including at least an obstacle distance cost reflecting the distance between the vehicle and the selected road boundary, a heading deviation cost reflecting the angle between the path and the desired driving direction, a curvature cost reflecting the magnitude of the path curvature, and a smoothness cost reflecting the rate of change of the path curvature; The path selection module is used to select the path with the minimum cost as the optimal path for the sanitation vehicle to travel close to the side of the road.

[0017] In the above technical solution, the boundary acquisition module includes a laser radar or camera sensor, which is used to detect the road boundary to obtain the road boundary point data. The device uses the real-time boundary information obtained by the sensor to perform edge-adapting path planning without high-precision map data.

[0018] The beneficial effects of the present invention are as follows: the present invention generates offset sampling points and constructs path clusters directly based on the perceived road boundary points. This method does not rely on offline maps and positioning information, and can plan a path that runs close to the edge even in scenarios where positioning is lost or there is no map, thereby greatly improving the robustness and applicability of the system. The present invention combines the actual scenario of sanitation vehicles cleaning close to the edge, and utilizes the characteristic that sanitation vehicles travel slowly during operation to simplify the target sampling points and reduce computing power. In the cost function calculation process, considering that the selected target path is a path that runs close to the edge, rather than an intermediate path, a special cost function calculation method is set for this purpose. In the cost function calculation process, considering that the sanitation vehicle has a slow speed during the cleaning process, the weight of path smoothing is reduced, and the cost weight of the target path and road boundary obstacles is increased, so that the selected optimal path can meet the purpose of cleaning close to the edge.

[0019] Furthermore, the present invention uses a polynomial curve algorithm to generate candidate paths, which can ensure that the curvature of each path is continuous and smooth, effectively avoid sharp turns or broken lines, and improve the driving smoothness and safety during the edge-to-edge driving process.

[0020] Furthermore, the present invention uniformly determines the longitudinal coordinates of each sampling point by presetting the foresight distance, and generates multiple sampling points along the boundary normal offset and lateral interval increments. This not only ensures the coverage of the sampling point distribution, but also effectively controls the sampling volume, balancing planning accuracy and computational overhead.

[0021] Furthermore, the present invention uses discrete sampling, minimum distance calculation and piecewise penalty function in the obstacle cost, which can accurately reflect the safe distance relationship between the vehicle and the road boundary, and exempt the cost when the safety threshold is exceeded, ensuring that neither collision occurs nor excessive conservatism.

[0022] Furthermore, the course deviation cost of the present invention is integrated by the angle between the path tangent and the desired forward direction, which can effectively constrain the candidate path yaw, maintain the heading consistency when the vehicle is driving close to the edge, and improve the accuracy of path tracking.

[0023] Furthermore, the curvature cost of the present invention increases the penalty for paths with large curvature by accumulating or integrating the square of the curvature, thereby screening out solutions with smoother curved paths and reducing the lateral acceleration and tire skidding risk of the vehicle when executing the edge-to-edge maneuver.

[0024] Furthermore, the smoothness cost of the present invention integrates and evaluates the curvature change rate, which can avoid sudden corner changes in the path, further improve driving smoothness, and reduce the impact on the vehicle chassis and occupants.

[0025] Furthermore, the present invention filters out adjacent points with too small a spacing during boundary point preprocessing, thereby removing perception noise and invalid redundancy, and improving the efficiency and stability of subsequent sampling and path generation.

[0026] Furthermore, the device described in the present invention modularizes the method steps into functional units such as boundary acquisition, processing, sampling, generation, calculation and selection. It has a clear structure and cooperates with each other. It can be directly integrated into the automatic driving controller of the sanitation vehicle, which is convenient for software and hardware implementation and system maintenance.

[0027] Furthermore, the present invention uses perception sensors such as lidar or cameras to obtain boundary information in real time, thereby achieving dynamic adaptation and rapid response in environments without high-precision maps, and can meet the edge cleaning needs of complex urban roads and newly paved roads. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 Schematic diagram of the method flow of the present invention; Figure 2 A schematic diagram of a road boundary in a specific embodiment; Figure 3 This is a schematic diagram of target point sampling in a specific embodiment; Figure 4 Schematic diagram of the shortest distance between a vehicle and a road boundary point in a specific embodiment. DETAILED DESCRIPTION

[0029] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments to facilitate a clear understanding of the present invention, but they do not constitute a limitation to the present invention.

[0030] Example 1 like Figure 1 As shown, the present invention provides a method for planning a path for a sanitation vehicle to follow the side of the road, comprising the following steps: Acquire road boundary points, and divide the road boundary points into left boundary points and right boundary points; Determine the side-to-side driving mode as left side or right side according to the upper layer instruction; Generate multiple target sampling points based on the road boundary point on the side indicated by the upper-layer instruction, wherein the target sampling points are located within a safety area after the road boundary point is offset along its normal direction toward the vehicle inner side by a preset safety distance; the vehicle inner side direction refers to a side direction starting from the road boundary point and along the outer normal of the point toward the vehicle center of mass (or the vehicle's lateral centerline) in the vehicle coordinate system; Taking the current position of the vehicle as the starting point, connecting the starting point with the target sampling point to generate a path cluster; Calculating the cost of each path in the path cluster according to a pre-designed cost function; the cost function includes multiple cost terms, including at least an obstacle distance cost reflecting the distance between the vehicle and the selected road boundary, a heading deviation cost reflecting the angle between the path and the desired driving direction, a curvature cost reflecting the magnitude of the path curvature, and a smoothness cost reflecting the rate of change of the path curvature; The path with the minimum cost is selected as the optimal path for the sanitation vehicle to travel close to the side of the road.

[0031] An embodiment of the present invention provides a method for planning a path for a sanitation vehicle to follow the edge of a road based on a drivable boundary, comprising the following steps: Perception Input and Boundary Acquisition: This process captures road boundary point data output by the perception system, representing the left and right edges of the road in the vehicle coordinate system. These boundary points are typically detected by sensors in front of the vehicle (such as lidar and cameras) and represent the boundaries of the drivable area of ​​the road.

[0032] Boundary point processing and classification: Figure 2 As shown, the acquired road boundary points are preprocessed and divided into a left boundary point set and a right boundary point set according to their distribution and longitudinal position in the vehicle coordinate system. At the same time, redundant points that are too dense or too close are deleted to reduce the impact of noise on path planning. The processed left and right boundary points clearly describe the boundary lines of the drivable areas on the left and right sides of the vehicle. Each set is then sorted in ascending order by the longitudinal coordinates to facilitate subsequent retrieval along the direction of vehicle travel. For each boundary point in the set, two normal directions are determined, and the normal direction facing the vehicle's center of mass is selected. The safety boundary point set is obtained by offsetting the preset safety distance. This set clearly describes the safety boundary lines inside the drivable areas on the left and right sides of the vehicle.

[0033] Target sampling point generation: Figure 3 As shown in the figure, starting from the vehicle's current position (typically set to the coordinate origin (0,0)), target sampling points are generated on the safety boundary point set according to the following rules. Sampling occurs in two directions: First, along the vehicle's forward direction (the x-axis), positions at a foresight distance x0 are selected. This foresight distance x0 is determined based on the current vehicle speed v and a preset time window t1. For example, t1 ≈ 6 s. Since sanitation vehicles operate at relatively low speeds (approximately 5–10 km / h), x0 = vt1, which is approximately 10–20 meters. In practice, to ensure a planned distance of at least one vehicle length, the lower limit of x0 can be set to 10 meters.

[0034] Select the safety boundary point with the vertical coordinate closest to x0 on the specified edge side (left / right), and mark its horizontal coordinate as y0 to obtain the starting target sampling point (x0, y0). Expand sampling laterally, increasing / decreasing inside the drivable area: , for the right side, k<0 means it is closer to the right side; for the left side, it is the opposite, thus combining several target sampling points {(x0,y k For example, when driving on the right side, turn inward around the right boundary line. Sample several target points for the step size, the blue sampling points are as follows Figure 3 As shown in Figure 2, the number of candidate trajectories is reduced through purposeful and regular sampling, thereby improving the efficiency of path planning.

[0035] Path cluster generation: Using a polynomial smooth interpolation algorithm, the starting point (0,0) is connected to each target sampling point to generate a cluster of smooth candidate paths. For example, a cubic or quintic polynomial curve fit can be used to ensure positional continuity and smooth transitions in the first-order derivative (heading angle) between the starting and target points. Each candidate path represents a feasible trajectory for the vehicle from its current position to the corresponding target point. The polynomial curve ensures continuous path curvature and no sharp curves, meeting the requirements for smooth driving of low-speed autonomous road sweepers. These paths collectively constitute a path cluster.

[0036] Cost function calculation: For each candidate path generated, calculate its path cost value. The cost function consists of multiple sub-items, including obstacle distance cost, heading deviation cost, curvature cost, and smoothness cost. First, set the focus direction of the cost function based on the boundary that the vehicle needs to stick to (left or right, specified by the upper layer instruction). Then, discrete sampling is performed on each path (the sampling interval is, for example, , such as 0.2 meters) to calculate the various costs. The total cost of the path is calculated by combining all the costs (weighted sum). Specific cost calculation details are described later.

[0037] Optimal Path Output: The total cost of each path in the path cluster is compared, and the path with the lowest cost is selected as the final optimal path that adheres to the road's edge. In other words, the present invention uses the cost function to select a path that is smooth and collision-free in the vehicle coordinate system. This optimal path enables the sanitation vehicle to adhere closely to the designated road boundary, meeting cleaning requirements. The final output path can be provided to the vehicle control system for tracking and execution.

[0038] The above process fully covers the entire process from sensor input, boundary processing, target point generation, path generation, cost evaluation, and finally the selection of the optimal path. The following is a detailed explanation of the cost function components and parameter selection.

[0039] During path planning, a cost function is introduced to evaluate candidate paths. This cost function comprehensively considers factors such as the distance between the vehicle and obstacles (road boundaries), driving course deviation, path curvature, and path smoothness. By properly setting each cost and its weight, the selected path can be ensured to meet the requirements of driving close to the edge while ensuring stable and safe driving. The cost function can be formally expressed as the weighted sum of each individual cost: Among them J obs is the obstacle distance cost, J heading is the cost of heading deviation, J curvature is the curvature cost, Js mooth is the smoothness (curvature change rate) cost, w1, w2, w3, w4 are the corresponding weight coefficients. The calculation details and functions of each sub-item cost are as follows: Obstacle distance cost: reflects the distance margin between the vehicle and the road boundary (obstacle) when traveling along the path.

[0040] First, based on the previously determined forward distance x0 and vehicle size, a set of valid boundary points within the path planning range is selected as the obstacle set. Specifically, the boundary points beyond the rear of the vehicle behind the current position (e.g. , where L1 is the distance from the rear axle center to the rear of the vehicle), and boundary points beyond the forward viewing distance (e.g. The remaining boundary points are recorded as the obstacle position set for subsequent distance calculation.

[0041] Then, each candidate path is divided into Discrete sampling is performed to obtain a series of sampling points. Based on the vehicle's dimensional model, the vehicle's outer contour at each sampling point (approximated as the coordinates of the eight vertices of a rectangle) is calculated. For each sampling location, the distances between the vehicle's edges and the nearest obstacle are calculated, and the minimum distance is recorded as d (representing the closest distance to the road edge at that vehicle's position).

[0042] The design algorithm of obstacle cost is as follows: Figure 4 As shown, if the vehicle needs to drive on the right side (keep to the right for cleaning), when the minimum distance d between the right side of the vehicle and the right boundary is r The minimum distance d between the left side of the vehicle and the left boundary is greater than the safety distance threshold of 0.15 meters. l When the distance is greater than the preset offset distance d0, the path is considered to meet the margin requirement and the obstacle cost is recorded as 0. Here, the safety distance (for example, 0.15 m) ensures that the vehicle does not collide with or scrape the curb; the offset distance d0 is determined by the road width w and the vehicle width w0, and is used to measure the degree of deviation of the vehicle from the other side of the boundary. In this embodiment, For example, if the road width w = 3.0 meters and the vehicle width w0 = 2.5 meters, then d0 = 3.0-2.5-0.15 = 0.35 meters, which means that when the vehicle is driving right next to the edge, there should be at least 35 centimeters between the left side and the left boundary.

[0043] If the above conditions are not met, the corresponding cost is assigned according to the insufficient distance: when the right side is d r When the path is less than 0.15 m, there is a dangerous edge contact or collision. obs Give a huge penalty (which can be regarded as an infeasible path for collision); when the right side is safe and the left side is d l When it is less than d0, it means that the vehicle is not driving close to the right side (deviating too far to the left), and a certain penalty needs to be given to make it tend to the edge. The obstacle cost can be expressed in the form of a piecewise function, such as: Where K1, K2 are the penalty constants set ( ), indicating that the risk of collision is greater than the penalty for not following the edge. This cost function ensures that the final selected path maintains a safety gap of about 15 cm on one side of the edge, while staying as close to the edge as possible on the other side without deviating from the center of the road. If the vehicle needs to drive on the left side (keep left to clean), the left and right distances are exchanged to obtain, Heading deviation cost: reflects the consistency between the vehicle's driving direction and the road boundary direction. To ensure that the sanitation vehicle travels parallel to the road boundary, it is necessary to minimize the path heading deviation. The heading deviation cost can be measured by the difference between the heading at the end of the path and the direction of the road boundary (or the initial direction of the vehicle). For example, assuming that the initial heading of the vehicle is parallel to the road boundary as the reference 0, the heading deviation of the path at the target sampling point is The larger the value, the higher the cost. ; where k h is the heading cost weight coefficient, Measured in radians. This cost penalizes paths where the vehicle's heading deviates significantly from the boundary (the sweeping direction) at the end, encouraging paths to remain parallel to the roadside during travel. If necessary, the angle between the heading and the road boundary can be integrated and evaluated throughout the entire path. However, since the scenarios in this invention mostly involve gently curved sections that approximate straight lines, the endpoint heading is primarily considered. By limiting the cost of heading deviation, it is possible to avoid planning trajectories where the vehicle's final heading is unfavorable for continued close-to-the-road travel.

[0044] Curvature cost: reflects the impact of path curvature on vehicle driving. Curvature indicates the sharpness of a path's turns. Excessive curvature can cause the vehicle to have difficulty turning or even be unable to track. To this end, the candidate path is represented by an arc length parameter s∈[0,L] as (x(s),y(s)). The first-order derivative of the arc length satisfies x ′ (s) 2 +y ′ (s) 2 = 1. The curvature can be written as: The second equal sign exploits the relationship parameterized by arc length.

[0045] The curvature cost is obtained by weighted integration of the square of the curvature of the entire path: Where L is the arc length of the path, k c is the curvature cost coefficient. The squared integral of curvature gives a higher penalty at places with larger curvatures, ensuring that the path turns as smoothly as possible. At the same time, the sum of the absolute values ​​of the curvatures of each sampling point can also be simply taken as an approximation. When implementing the project, the path can be discretized according to the longitudinal step size and Approximate the curvature of each sampling point, and then use Numerical quadrature is performed, which is simple and efficient. Regardless of the form adopted, the curvature cost is intended to limit sharp curves. The curvature of the planned path should be within the vehicle's turning capability and as small as possible to improve driving safety and ride comfort.

[0046] Smoothness cost: Smoothness cost reflects the speed of change of curvature with driving distance, suppressing the control discontinuity caused by "sudden change". Define the first derivative of curvature with respect to arc length: The smoothness cost is the integral of the square of the curvature change rate: where k s is the smoothness weight. When implemented discretely, the curvature of adjacent sampling points can be differentiated: Press again: Accumulate the sum. If the calculation is performed under the analytical representation y(x), it can also be used: Perform the substitution to ensure that the same independent variable system is used as the curvature cost. Since the sanitation vehicle has a low operating speed (about 5-10 km / h), this embodiment usually takes k s <k c This reduces sensitivity to minor oscillations while still suppressing abrupt turns, balancing safe edge tracking with computational efficiency. A lower smoothness cost indicates a more gradual change in path curvature, resulting in smoother, more gradual vehicle steering and fewer sudden, drastic changes in direction. Therefore, this invention assigns a lower weight to the smoothness cost, while placing the primary weight on the obstacle distance cost. This ensures safe edge tracking without deviating from cleaning requirements due to an excessive pursuit of extreme smoothness.

[0047] By combining the weighted cost functions described above, the path cost function of this invention comprehensively evaluates the quality of candidate paths. Even when the vehicle lacks localization or map reliance, it can still plan a close-to-the-edge path based on real-time boundary information. Furthermore, to account for the specific requirements of cleaning operations (close to the curb and tight safety gaps), the cost function prioritizes obstacle distance and reduces the weighting of smoothness. Ultimately, path selection is based on minimizing total cost, automatically selecting the optimal path that meets the requirements for close-to-the-edge cleaning.

[0048] When implementing the above method, the selection of key parameters has a significant impact on the effect. This embodiment provides some recommended parameter settings and their reasons based on the actual scenario of sanitation sweepers: Look-ahead distance x0: determines the longitudinal position of the target sampling point. x0 should be selected based on vehicle speed to ensure that the planned path is sufficiently forward-looking. It is usually calculated by vt1, where t1 is the preview time window. Sanitation vehicles operate at a relatively slow speed (e.g., 5-10 km / h), so t1≈6 seconds is more appropriate. At the same time, in order to cover a distance of at least one vehicle length, this embodiment limits x0 to be greater than or equal to 10 meters. A larger x0 provides a longer planned path, but too large an x0 increases the amount of calculation; too small an x0 results in a short path that requires frequent replanning. Therefore, a compromise should be made based on vehicle speed and operating conditions.

[0049] Vehicle dimensional parameters include the distance from the center of the rear axle to the front of the vehicle (L0), the distance from the center of the rear axle to the rear of the vehicle (L1), and the vehicle width (w0). In this example, the sanitation sweeper is approximately 8 meters long (wheelbase and front and rear overhangs combined), so L0 and L1 can be approximately 4 meters each; the vehicle width (w0) is approximately 2.5 meters. These dimensional parameters directly influence the determination of drivable space and the calculation of safe distance. For example, L1 is used to filter out boundary obstacles behind the vehicle, while L0 is used to filter out boundary points that are too far forward. The width is used to determine the offset threshold (d0) on the other side when driving close to the edge.

[0050] Minimum safety distance: that is, the minimum lateral clearance maintained between the vehicle and the road boundary. During cleaning operations, it is necessary to get as close to the curb as possible to improve cleaning coverage, but collisions must also be avoided. The present invention sets the safety gap threshold to 0.15 meters (about 15 centimeters). This distance can be adjusted according to the accuracy of the vehicle sensor and the operational safety margin. In this embodiment, 15 cm is selected mainly considering the typical curb height and the vehicle's avoidance ability. If you get closer within this distance, there is a risk of scratching. Therefore, the obstacle cost increases sharply when the distance is less than 0.15 m to exclude such paths.

[0051] Normal offset distance d0: This refers to the target clearance between the vehicle's left side and the opposite boundary when the vehicle is driving close to one side. Its value depends on the difference between the road width w and the vehicle width w0, as well as an additional safety margin. For example, if the width between the two road boundaries is w and the vehicle width is w0, and the desired distance between the right side of the vehicle and the right boundary is 0.15 meters, the theoretical remaining clearance on the left side is w - w0 - 0.15, which is d0. In this example, assuming w is approximately 3 meters, d0 ≈ 0.35 meters. The offset distance measures whether the vehicle is sufficiently close to the boundary: if the left side of the vehicle is less than d0 from the left boundary, it is not close enough to the right side, and an additional penalty is required to encourage it to move closer to the boundary. Conversely, if it is greater than d0, it is considered close enough and no additional penalty is required. d0 needs to be adjusted accordingly for different road widths, and generally should be slightly less than w - w0 to ensure a safe clearance on the side of the vehicle.

[0052] Sampling interval: including longitudinal sampling interval (path discrete steps) and lateral sampling intervals (Horizontal step length of target point). When using discrete sampling points for path evaluation, both accuracy and efficiency should be considered. If the value is too large, details such as narrow obstacles may be missed, while if it is too small, the calculation will be cumbersome. The sampling interval can be 0.1-0.5 meters, which is selected according to the size of the vehicle, so that the vehicle model (about several meters long) can detect a potential collision by moving one step. Determines the density of the generated target points. If the boundary line is relatively straight, a larger Use a small number of target points to cover possible trajectories; if the boundary is tortuous or needs to be more precise, Take a smaller value to generate more candidate points close to the boundary. It can be set in the range of 0.1-0.5 meters according to the cleaning width or the desired edge distance.

[0053] The above parameter settings can be adjusted based on the specific vehicle and environmental requirements. By rationally setting these parameters, the present invention improves computational efficiency and adaptability while ensuring path planning reliability. For example, the slow speed and short paths of sanitation vehicles allow for a smaller smoothness weight and a slightly coarser sampling step size, simplifying calculations without compromising safety. Conversely, parameters such as the obstacle safety distance are set relatively strictly to ensure both edge alignment and safety.

[0054] Example 2 The present invention provides a path planning device for an automatic driving sanitation vehicle, which is used to implement the method described in the above technical solution, including: A boundary acquisition module, used to obtain road boundary point data in the vehicle's surrounding environment; A boundary processing module is used to pre-process the acquired boundary point data and divide it into a left boundary point set and a right boundary point set according to the distribution of the boundary points in the vehicle coordinate system; A target sampling module is configured to generate a plurality of target sampling points based on the road boundary point on the side indicated by the upper-layer instruction, wherein the target sampling points are located in a safety area after the road boundary point is offset toward the vehicle inner side by a preset safety distance along its normal direction; a path generation module, configured to connect the vehicle's current position with each of the target sampling points using a polynomial smooth curve algorithm to generate a plurality of candidate driving paths; a cost calculation module, configured to calculate the cost of each path in the path cluster according to a pre-designed cost function; the cost function comprising a plurality of cost items, including at least an obstacle distance cost reflecting the distance between the vehicle and the selected road boundary, a heading deviation cost reflecting the angle between the path and the desired driving direction, a curvature cost reflecting the magnitude of the path curvature, and a smoothness cost reflecting the rate of change of the path curvature; The path selection module is used to select the path with the minimum cost as the optimal path for the sanitation vehicle to travel close to the side of the road.

[0055] The path planning device of this embodiment can be integrated into the automatic driving control system of the sanitation vehicle. It is composed of multiple functional modules. The modules work together in the order of data flow to realize the above-mentioned edge driving path planning.

[0056] Specifically, the Boundary Acquisition Module receives and acquires road boundary point information output by the Perception Layer. This module interfaces with the vehicle's environmental perception sensors (such as LiDAR and cameras) or environmental model data provided by a host computer, converting the detected road boundary coordinates into the vehicle coordinate system for output.

[0057] Specifically, the boundary processing module processes and classifies the acquired boundary point set. This module divides the boundary points into two groups, left and right, based on their lateral coordinates and filters out redundant points (e.g., points that are too close or noisy). This processed boundary data is smoother and more reliable, and it identifies the left and right boundary outlines of the vehicle's current lane.

[0058] Specifically, the command parsing module is responsible for obtaining and parsing upper-level control commands, determining whether the vehicle should drive to the left or right edge of the road. These upper-level commands may originate from the mission plan (e.g., an instruction to drive to the right curb for cleaning). This module transmits these commands to the subsequent path planning process, prioritizing sampling and cost calculation.

[0059] Specifically, the target sampling module generates a set of target sampling points. Based on the current vehicle position (typically the center of the rear axle is used as the origin) and boundary geometry information from the boundary processing module, the module samples candidate target points at a predetermined distance ahead, along the boundary on the side specified by the instruction. This sampling uses a preset longitudinal distance and lateral step size (sampling only within the lane on the specified side), resulting in a set of target point coordinates. Each target point represents the endpoint of a possible target path for close-to-edge driving.

[0060] Specifically, the path generation module is used to generate a smooth path cluster based on the starting point and target sampling points. For each target point, this module calculates a smooth trajectory from the vehicle's current position to the target. Polynomial curve fitting techniques are preferably used to ensure smooth and continuous paths, avoiding collisions and sudden initial turns. All generated paths are combined to form a path cluster, which is passed to the next module via a data interface.

[0061] Specifically, the cost calculation module is used to calculate the cost function value for each path in the path cluster. The module obtains the edge information from the instruction parsing module to set the obstacle cost focus direction, then discretizes the path according to the predetermined sampling interval, and calculates the closest distance between the vehicle and the boundary from the valid boundary point set obtained from the boundary processing module, and calculates the obstacle distance cost based on this. 。 At the same time, the heading angle deviation, curvature and curvature change rate are calculated according to the path curve, and the corresponding J is calculated. heading , J curvature , J smooth 。 Finally, the weighted sum is used to get the total cost. This module stores and sorts the cost of each path.

[0062] Specifically, the path selection module compares the costs of all candidate paths and selects the optimal one. The module obtains the cost of each path from the cost calculation module and selects the path with the lowest cost as its output. If multiple paths with similar costs exist, other strategies (such as selecting a more direct path) may be combined to make the decision. Ultimately, the selected optimal path is sent to the vehicle's trajectory tracking control system for execution.

[0063] The above-mentioned functional modules can be implemented by combining software and hardware, such as by the software program of the on-board computer calling the sensor interface and performing calculations to complete the corresponding functions; or they can be divided into independent hardware units. For example, the boundary acquisition module can be implemented by a dedicated perception processing unit, and the path generation and cost calculation module can be implemented by a GPU-accelerated computing unit. The modules are connected in series according to the data flow sequence, and their input and output relationships are as described above, forming a complete edge-to-edge path planning device. From a system perspective, the device effectively realizes the function of planning edge-to-edge cleaning paths without the need for high-precision maps and even when positioning is unreliable, and has strong practicality and robustness.

[0064] Example 3 The path planning method of the present invention can also be implemented by a computer program. The program can be stored in a computer-readable storage medium, such as ROM, RAM, hard disk, flash disk or optical disk. When the program is executed by the processor, the various steps of the above-mentioned edge-to-edge driving path planning method can be executed, including processes such as perception data processing, target point sampling, path generation, cost calculation, and optimal path selection. That is to say, after the program instructions stored on the medium are loaded into the on-board computer, it will be operated according to the logic of the method of the present invention, and a driving path that meets the requirements of the sanitation vehicle for edge cleaning will be planned, and it can be further linked with the vehicle control system interface to guide the vehicle to travel along the planned path. By adopting the form of a computer-readable storage medium, the technical solution of the present invention can be conveniently deployed in the existing sanitation vehicle automatic driving system to achieve software-level upgrade and reuse, and has good versatility and scalability.

[0065] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Thus, the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to magnetic disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0066] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowcharts and / or block diagrams, as well as combinations of processes and / or blocks in the flowcharts and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowcharts and / or block diagrams. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0067] These computer program instructions may also be stored in a computer readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0068] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0069] The embodiments of the present invention are described above in conjunction with the accompanying drawings, but the present invention is not limited to the above-mentioned specific implementation methods. The above-mentioned specific implementation methods are merely illustrative and not restrictive. Under the guidance of the present invention, ordinary technicians in this field can also make many forms without departing from the scope of protection of the present invention and the claims, all of which are protected by the present invention.

[0070] The contents not described in detail in this specification belong to the prior art known to those skilled in the art.

Claims

1. A method for planning a path for a sanitation vehicle to follow the side of the road, characterized in that: The following steps are involved: Acquire road boundary points, and divide the road boundary points into left boundary points and right boundary points; Determine the side-to-side driving mode as left side or right side according to the upper layer instruction; Generate multiple target sampling points based on the road boundary point on the side indicated by the upper-level instruction, wherein the target sampling points are located in a safety area after the road boundary point is offset toward the vehicle inner side by a preset safety distance along its normal direction; Taking the current position of the vehicle as the starting point, connecting the starting point with the target sampling point to generate a path cluster; Calculating the cost of each path in the path cluster according to a pre-designed cost function; the cost function includes multiple cost terms, including at least an obstacle distance cost reflecting the distance between the vehicle and the selected road boundary, a heading deviation cost reflecting the angle between the path and the desired driving direction, a curvature cost reflecting the magnitude of the path curvature, and a smoothness cost reflecting the rate of change of the path curvature; The path with the minimum cost is selected as the optimal path for the sanitation vehicle to travel close to the side of the road.

2. The method according to claim 1, wherein: The current position of the vehicle is connected to each of the target sampling points using a polynomial curve algorithm to generate candidate driving paths that meet the requirements of curvature continuity and path smoothness as a path cluster.

3. The method according to claim 1, wherein: The process of generating the target sampling points includes: The vehicle's current position is used as the origin of the vehicle coordinate system, the vehicle's forward direction is used as the longitudinal axis, and a preset foresight distance is set on the axis. The preset foresight distance is used as the longitudinal coordinate of all target sampling points. Obtaining a boundary point located at or near the longitudinal coordinate and belonging to the road boundary side specified by the upper-level instruction; The preset safety distance is offset inward of the vehicle's drivable area along the normal direction of the boundary point, and the lateral coordinate of the offset position is used as the lateral coordinate of the starting target sampling point; The horizontal coordinates of the remaining target sampling points are obtained by increasing or decreasing the horizontal coordinates of the initial target sampling point at preset horizontal intervals.

4. The method according to claim 1, wherein: The calculation of the obstacle distance cost includes: Discretely sample the vehicle trajectory at a preset longitudinal sampling interval on each candidate path; At each sampling point, the minimum distance between the vehicle and the road boundary is calculated based on the vehicle's outer contour and the set of valid road boundary points; Inputting the minimum distance into a monotonically decreasing penalty function to obtain a local distance penalty value, and summing the penalty values ​​of all sampling points to obtain the obstacle distance cost of the candidate path; When the minimum distance is greater than a preset safety threshold, the corresponding local penalty value is set to zero, so that the cost is not increased when the safety distance is met.

5. The method according to claim 1, wherein: The heading deviation cost is obtained by calculating the angle between the tangential direction of the candidate path and the desired driving direction of the vehicle at each sampling point, and integrating or accumulating the absolute value or square of the angle along the entire path.

6. The method according to claim 1, wherein: The curvature cost is obtained by discretely sampling the curvature value along the candidate path and accumulating or integrating the square of the curvature at each sampling point, so as to penalize the path with a larger curvature.

7. The method according to claim 1, wherein: The smoothness cost is obtained by calculating the difference in curvature values ​​between adjacent sampling points to obtain the curvature change rate, and accumulating or integrating the square of the curvature change rate over the entire candidate path to penalize paths with abrupt curvature changes, thereby ensuring driving smoothness.

8. The method according to claim 1, wherein: When processing the road boundary points, adjacent road boundary points whose distances are less than a preset distance threshold are filtered out.

9. A sanitation vehicle automatic driving path planning device, characterized in that: For implementing the method according to any one of claims 1 to 8, comprising: A boundary acquisition module, used to obtain road boundary point data in the vehicle's surrounding environment; A boundary processing module is used to pre-process the acquired boundary point data and divide it into a left boundary point set and a right boundary point set according to the distribution of the boundary points in the vehicle coordinate system; A target sampling module is configured to generate a plurality of target sampling points based on the road boundary point on the side indicated by the upper-layer instruction, wherein the target sampling points are located in a safety area after the road boundary point is offset toward the vehicle inner side by a preset safety distance along its normal direction; a path generation module, configured to connect the vehicle's current position with each of the target sampling points using a polynomial smooth curve algorithm to generate a plurality of candidate driving paths; a cost calculation module, configured to calculate the cost of each path in the path cluster according to a pre-designed cost function; the cost function comprising a plurality of cost items, including at least an obstacle distance cost reflecting the distance between the vehicle and the selected road boundary, a heading deviation cost reflecting the angle between the path and the desired driving direction, a curvature cost reflecting the magnitude of the path curvature, and a smoothness cost reflecting the rate of change of the path curvature; The path selection module is used to select the path with the minimum cost as the optimal path for the sanitation vehicle to travel close to the side of the road.

10. The device according to claim 9, characterized in that: The boundary acquisition module includes a laser radar or camera sensor, which is used to detect the road boundary to obtain the road boundary point data. The device uses the real-time boundary information obtained by the sensor to perform edge-fitting path planning without high-precision map data.

Images