An intelligent cleaning path planning method for a sanitation vehicle based on urban roads

Abstract

The application discloses a kind of based on urban road's intelligent cleaning path planning method of sanitation vehicle, and the road edge and obstacle profile in the working environment of sanitation vehicle are identified by laser radar sensor and binocular camera sensor, and the positioning deviation is corrected using extended Kalman filter fusion IMU data, obtain high-precision map and dynamic traffic data, and energy consumption hot spot area is marked in road topology diagram;Using LPV-MPC control model, the load change and brake delay of vehicle when working in the first working path of sanitation vehicle are compensated by fusing Smith predictor, the yaw angle is adjusted based on adaptive PID algorithm, and the path tracking error model is fed back to MPC controller;If it can be passed, output uniform speed travel instruction, otherwise generate steering instruction.The accurate control of vehicle driving direction and speed is realized.The sanitation vehicle can stably travel under various working conditions, accurately track the planned path, and improve the quality and effect of cleaning operation.

Description

Technical Field

[0001] This invention relates to the field of path planning technology, and in particular to a method for intelligent sweeping path planning for sanitation vehicles based on urban roads. Background Technology

[0002] Traditional sanitation vehicle cleaning operations often rely on manual experience for route planning, which is not only inefficient but also difficult to adapt to the complex and ever-changing urban traffic environment. During operation, sanitation vehicles lack precise environmental perception and positioning capabilities, making it difficult to accurately identify road edges and obstacles, increasing the risk of collisions. Existing route planning methods mostly only consider the shortest path or the shortest time, ignoring energy consumption factors and the impact of dynamic traffic conditions. Sanitation vehicles frequently pass through energy-intensive areas, resulting in significant energy waste; when encountering traffic lights, they often fail to plan their speed appropriately, leading to prolonged waits at intersections and reducing cleaning efficiency. Summary of the Invention

[0003] The purpose of this invention is to solve the above problems by designing an intelligent sweeping path planning method for sanitation vehicles based on urban roads.

[0004] To achieve the above objectives, the technical solution of the present invention further includes the following steps in the above-mentioned intelligent cleaning path planning method for sanitation vehicles based on urban roads:

[0005] The road edges and obstacle contours in the working environment of the sanitation vehicle are identified by LiDAR sensor and binocular camera sensor. The positioning deviation is corrected by using extended Kalman filter to fuse IMU data and obtain the initial pose data of the sanitation vehicle.

[0006] High-precision maps and dynamic traffic data are acquired, energy consumption hotspots are marked on the road topology map, an initial path is generated using an improved A* algorithm, and then the Pareto front solution is optimized using a genetic algorithm to obtain the first working path of the sanitation vehicle.

[0007] Using the LPV-MPC control model, the Smith predictor is integrated to compensate for the load changes and braking delay of the vehicle when it is working on the first working path of the sanitation vehicle. The yaw angle is adjusted based on the adaptive PID algorithm, and the path tracking error model is fed back to the MPC controller to obtain the second working path of the sanitation vehicle.

[0008] When a vehicle approaches a traffic light, the green wave speed calculation module is activated. If the prediction is successful, a constant speed driving command is output; otherwise, a steering command is generated to obtain the target working path of the sanitation vehicle.

[0009] Furthermore, in the aforementioned intelligent sweeping path planning method for sanitation vehicles based on urban roads, the step of identifying road edges and obstacle contours in the working environment of the sanitation vehicle using a lidar sensor and a binocular camera sensor includes:

[0010] By measuring the time difference between the emission and reception of a laser pulse using a lidar sensor, and combining the laser emission angle and velocity, the distance between the object and the sensor is calculated to obtain three-dimensional point cloud data.

[0011] The three-dimensional point cloud data is processed, and a clustering algorithm is used to distinguish road edges and obstacles;

[0012] Image data is acquired using a binocular camera sensor. The images captured by the left and right cameras are corrected and matched to obtain the corresponding points of the same object in the left and right images. The depth information of the object is calculated according to the parallax formula to obtain the three-dimensional coordinate data of the object.

[0013] The three-dimensional coordinate data is processed to identify the specific shapes and locations of road edges and obstacles, thereby obtaining the initial environmental data of the sanitation vehicle.

[0014] Furthermore, in the aforementioned intelligent sweeping path planning method for sanitation vehicles based on urban roads, the step of using extended Kalman filtering to fuse IMU data to correct positioning deviations and obtain initial pose data for the sanitation vehicle includes:

[0015] Based on the pose estimate from the previous moment and the acceleration and angular velocity measured by the IMU, the pose estimate for the current moment is predicted using a kinematic model, including at least position, velocity, and attitude angle.

[0016] Acquire initial environmental data of the sanitation vehicle, match the initial environmental data of the sanitation vehicle with the pose estimation value, convert the pose estimation value into the observation value through the measurement model, calculate with the actual observation value to obtain the observation residual;

[0017] By incorporating the observation residuals into the pose estimate using the gain matrix of the extended Kalman filter, the error covariance matrix is ​​updated to obtain the initial pose data of the sanitation vehicle.

[0018] Furthermore, in the aforementioned intelligent sweeping path planning method for sanitation vehicles based on urban roads, the steps of acquiring high-precision maps and dynamic traffic data, marking energy consumption hotspots in the road topology map, and generating an initial path using an improved A* algorithm include:

[0019] Acquire high-precision maps and dynamic traffic data, and generate a road topology map for sanitation vehicle operations based on the high-precision maps;

[0020] Based on the dynamic traffic data, energy consumption hotspots are marked on the road topology map, including at least road slope, traffic flow, and road curvature.

[0021] The nodes in the road topology graph are defined as the endpoints of road segments, and the state of the nodes includes at least position and orientation;

[0022] The cost function is calculated, including the actual cost from the starting point to the current node and the heuristic cost from the current node to the destination. The node with the minimum cost function is selected through a priority queue for expansion, and the destination node is found to obtain the initial path.

[0023] Furthermore, in the aforementioned intelligent sweeping path planning method for sanitation vehicles based on urban roads, the step of further optimizing the Pareto front solution using a genetic algorithm to obtain the first working path of the sanitation vehicle includes:

[0024] Generate an initial set of paths as a population. Each path is represented as a chromosome, and the genes of the chromosome are the sequence of nodes traversed by the path.

[0025] Define a fitness function, combine path length, energy consumption and travel time, use Pareto dominance to evaluate the quality of individuals, and retain non-dominated solutions as Pareto front solutions;

[0026] Individuals with high fitness were selected as parents, and the population was gradually brought closer to the Pareto front through genetic manipulation and iteration to obtain the first working path of the sanitation vehicle.

[0027] Furthermore, in the aforementioned intelligent sweeping path planning method for sanitation vehicles based on urban roads, the method utilizes an LPV-MPC control model, integrates a Smith predictor to compensate for load changes and braking delays during the vehicle's first working path, adjusts the yaw angle based on an adaptive PID algorithm, and feeds back the path tracking error model to the MPC controller to obtain the sanitation vehicle's second working path, including:

[0028] An LPV-MPC control model for sanitation vehicles was established, with load and braking delay parameters used as time-varying parameters.

[0029] Based on the current parameter estimates and future predicted inputs, the optimal control sequence is calculated. The Smith predictor is used to compensate for load changes and braking delays of the sanitation vehicle during operation, thus obtaining the second working path of the sanitation vehicle.

[0030] Furthermore, in the aforementioned intelligent sweeping path planning method for sanitation vehicles based on urban roads, the step of activating the green wave speed calculation module when the vehicle approaches a traffic light, outputting a constant speed driving command if the prediction is successful, and generating a steering command otherwise, to obtain the target working path of the sanitation vehicle, includes:

[0031] The system obtains the cycle duration, current signal light status, and remaining time of the traffic light ahead by using dynamic traffic data; calculates the distance from the vehicle's current location to the traffic light intersection; and, combined with the remaining time of the traffic light, obtains the signal light status when the vehicle arrives at the intersection.

[0032] Based on the traffic light status, a prediction is made. If the prediction is successful, a constant speed driving command is output; otherwise, a turning command is generated to obtain the target working path of the sanitation vehicle.

[0033] Furthermore, in the system for implementing the above-mentioned intelligent sweeping path planning method for sanitation vehicles based on urban roads, the system includes the following modules:

[0034] The pose data acquisition module is used to identify the road edges and obstacle contours in the working environment of the sanitation vehicle through the lidar sensor and the binocular camera sensor, and to correct the positioning deviation by using extended Kalman filter to fuse IMU data to obtain the initial pose data of the sanitation vehicle.

[0035] The work path planning module is used to acquire high-precision maps and dynamic traffic data, mark energy consumption hotspots in the road topology map, generate an initial path using an improved A* algorithm, and then optimize the Pareto front solution using a genetic algorithm to obtain the first work path of the sanitation vehicle.

[0036] The working path optimization module is used to utilize the LPV-MPC control model, integrate the Smith predictor to compensate for the load changes and braking delays of the vehicle when it is working on the first working path of the sanitation vehicle, adjust the yaw angle based on the adaptive PID algorithm, and feed back to the MPC controller through the path tracking error model to obtain the second working path of the sanitation vehicle.

[0037] The sanitation vehicle avoidance module is used to activate the green wave speed calculation module when the vehicle approaches the traffic light. If the prediction is that it can pass, it outputs a constant speed driving command; otherwise, it generates a steering command to obtain the target working path of the sanitation vehicle.

[0038] Furthermore, in the system for implementing the above-mentioned intelligent sweeping path planning method for sanitation vehicles based on urban roads, the system includes the following modules:

[0039] A submodule is established to build the LPV-MPC control model of the sanitation vehicle, using load and braking delay parameters as time-varying parameters;

[0040] The submodule is obtained to calculate the optimal control sequence based on the current parameter estimates and future predicted inputs. The Smith predictor is used to compensate for load changes and braking delays of the sanitation vehicle during operation, and the second working path of the sanitation vehicle is obtained.

[0041] Furthermore, in the system for implementing the above-mentioned intelligent sweeping path planning method for sanitation vehicles based on urban roads, the system includes the following modules:

[0042] The acquisition submodule is used to obtain the cycle duration, current signal light status, and remaining time of the traffic light ahead through dynamic traffic data; calculate the distance from the vehicle's current position to the traffic light intersection, and obtain the signal light status when the vehicle arrives at the intersection by combining the remaining time of the traffic light;

[0043] The prediction submodule is used to make predictions based on the traffic light status. If the prediction is successful, it outputs a constant speed driving command; otherwise, it generates a steering command to obtain the target working path of the sanitation vehicle.

[0044] Its beneficial effects are as follows: By using a LiDAR sensor and a binocular camera sensor to identify road edges and obstacle contours in the sanitation vehicle's working environment, and using an extended Kalman filter to fuse IMU data to correct positioning deviations, the initial pose data of the sanitation vehicle is obtained. High-precision maps and dynamic traffic data are acquired, energy-intensive hotspots are marked on the road topology map, and an improved A* algorithm is used to generate an initial path. Then, a genetic algorithm is used to optimize the Pareto front solution, resulting in the first working path of the sanitation vehicle. Using an LPV-MPC control model, a Smith predictor is fused to compensate for load changes and braking delays during the first working path of the sanitation vehicle. The yaw angle is adjusted based on an adaptive PID algorithm, and the path tracking error model is fed back to the MPC controller to obtain the second working path of the sanitation vehicle. When the vehicle approaches a traffic light, the green wave speed calculation module is activated. If the prediction is successful, a constant speed driving command is output; otherwise, a steering command is generated, resulting in the target working path of the sanitation vehicle. 1. This enables sanitation vehicles to clearly perceive their surroundings in complex urban road environments, avoiding collisions, while ensuring comprehensive cleaning coverage and eliminating blind spots. 2. It not only considers path length but also incorporates multiple objectives such as energy consumption and travel time into the optimization scope. While ensuring the completion of cleaning tasks, it effectively reduces the energy consumption of sanitation vehicles, improves driving efficiency, and reduces operating time. 3. It achieves precise control over the vehicle's direction and speed. This allows sanitation vehicles to drive stably under various working conditions, accurately tracking planned routes and improving the quality and effectiveness of cleaning operations. 4. It can intelligently plan vehicle speed or adjust driving routes based on traffic light status and the vehicle's own conditions. This avoids long waits at red light intersections, reduces the number of stops and starts, further improves operational efficiency, and simultaneously reduces vehicle wear and energy consumption. Attached Figure Description

[0045] Various other advantages and benefits will become apparent to those skilled in the art upon reading the following detailed description of preferred embodiments. The accompanying drawings are for illustrative purposes only and are not intended to limit the invention.

[0046] Figure 1This is a schematic diagram of the first embodiment of an intelligent sweeping path planning method for sanitation vehicles based on urban roads according to an embodiment of the present invention;

[0047] Figure 2 This is a schematic diagram of a second embodiment of an intelligent cleaning path planning method for sanitation vehicles based on urban roads according to an embodiment of the present invention;

[0048] Figure 3 This is a schematic diagram of the first embodiment of a cloud-based mapping data processing system according to the present invention. Detailed Implementation

[0049] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0050] Those skilled in the art will understand that, unless specifically stated otherwise, the singular forms “a,” “an,” “the,” and “the” used herein may also include the plural forms. It should be further understood that the term “comprising” as used in this specification means the presence of features, integers, steps, operations, elements, and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and / or groups thereof.

[0051] The present invention will now be described in detail with reference to the accompanying drawings, such as... Figure 1 As shown, a method for intelligent cleaning path planning for sanitation vehicles based on urban roads is described. This method includes the following steps:

[0052] Step 101: Identify the road edges and obstacle outlines in the working environment of the sanitation vehicle using the lidar sensor and the binocular camera sensor, and use extended Kalman filter to fuse IMU data to correct the positioning deviation and obtain the initial pose data of the sanitation vehicle.

[0053] Specifically, in this embodiment, the time difference between the emission and reception of a laser pulse is measured by a lidar sensor. Combined with the laser emission angle and velocity, the distance between the object and the sensor is calculated to obtain three-dimensional point cloud data.

[0054] Data processing is performed on 3D point cloud data, and clustering algorithms are used to distinguish road edges and obstacles;

[0055] Image data is acquired using a binocular camera sensor. The images captured by the left and right cameras are corrected and matched to obtain the corresponding points of the same object in the left and right images. The depth information of the object is calculated according to the parallax formula to obtain the three-dimensional coordinate data of the object.

[0056] The three-dimensional coordinate data is processed to identify the specific shapes and locations of road edges and obstacles, thus obtaining the initial environmental data of the sanitation vehicle.

[0057] Based on the pose estimate from the previous moment and the acceleration and angular velocity measured by the IMU, the pose estimate for the current moment is predicted using a kinematic model, including at least position, velocity, and attitude angle.

[0058] Acquire initial environmental data of the sanitation vehicle, match the initial environmental data of the sanitation vehicle with the pose estimation value, convert the pose estimation value into the observation value through the measurement model, calculate with the actual observation value, and obtain the observation residual;

[0059] By using the gain matrix of the extended Kalman filter, the observation residuals are incorporated into the pose estimate, and the error covariance matrix is ​​updated to obtain the initial pose data of the sanitation vehicle.

[0060] Step 102: Obtain high-precision map and dynamic traffic data, mark energy consumption hotspot areas in the road topology map, generate the initial path using the improved A* algorithm, and then optimize the Pareto front solution using the genetic algorithm to obtain the first working path of the sanitation vehicle.

[0061] Specifically, in this embodiment, high-precision maps and dynamic traffic data are acquired, and a road topology map of the sanitation vehicle's operation is generated based on the high-precision map.

[0062] Based on dynamic traffic data, mark energy consumption hotspots in the road topology map, including at least road slope, traffic flow, and road curvature.

[0063] Define the nodes in the road topology graph as the endpoints of road segments, and the state of a node includes at least its position and orientation;

[0064] The cost function is calculated, including the actual cost from the starting point to the current node and the heuristic cost from the current node to the destination. The node with the minimum cost function is selected through a priority queue for expansion, and the destination node is found to obtain the initial path.

[0065] Step 103: Using the LPV-MPC control model, the Smith predictor is integrated to compensate for the load changes and braking delay of the vehicle when it is working on the first working path of the sanitation vehicle. The yaw angle is adjusted based on the adaptive PID algorithm and fed back to the MPC controller through the path tracking error model to obtain the second working path of the sanitation vehicle.

[0066] Specifically, in this embodiment, a set of initial paths is generated as a population, each path is represented as a chromosome, and the genes of the chromosome are the node sequences passed through by the path;

[0067] Define a fitness function, combine path length, energy consumption and travel time, use Pareto dominance to evaluate the quality of individuals, and retain non-dominated solutions as Pareto front solutions;

[0068] Individuals with high fitness were selected as parents, and the population was gradually brought closer to the Pareto front through genetic manipulation and iteration to obtain the first working path of the sanitation vehicle.

[0069] An LPV-MPC control model for sanitation vehicles was established, with load and braking delay parameters used as time-varying parameters.

[0070] Based on the current parameter estimates and future predicted inputs, the optimal control sequence is calculated. The Smith predictor is used to compensate for load changes and braking delays of the sanitation vehicle during operation, thus obtaining the second working path of the sanitation vehicle.

[0071] Step 104: When the vehicle approaches the traffic light, the green wave speed calculation module is activated. If the prediction is successful, a constant speed driving command is output; otherwise, a steering command is generated to obtain the target working path of the sanitation vehicle.

[0072] Specifically, in this embodiment, the cycle duration, current signal light status, and remaining time of the traffic light ahead are obtained through dynamic traffic data; the distance from the vehicle's current position to the traffic light intersection is calculated, and the signal light status when the vehicle arrives at the intersection is obtained by combining the remaining time of the traffic light.

[0073] Based on the traffic light status, a prediction is made. If the prediction is successful, a constant speed driving command is output; otherwise, a turning command is generated to obtain the target working path of the sanitation vehicle.

[0074] Its beneficial effects are as follows: 1. It enables sanitation vehicles to clearly perceive their surroundings in complex urban road environments, avoiding collisions while ensuring comprehensive cleaning coverage and eliminating blind spots. 2. It not only considers route length but also incorporates multiple objectives such as energy consumption and travel time into the optimization scope. While ensuring the completion of cleaning tasks, it effectively reduces the energy consumption of sanitation vehicles, improves driving efficiency, and reduces operating time. 3. It achieves precise control of vehicle direction and speed, enabling sanitation vehicles to drive stably under various working conditions, accurately track planned routes, and improve the quality and effectiveness of cleaning operations. 4. It can intelligently plan vehicle speed or adjust driving routes based on traffic light status and vehicle conditions. This avoids long waits at red light intersections, reduces the number of stops and starts, further improves operational efficiency, and reduces vehicle wear and energy consumption.

[0075] Please see Figure 2 In a method for intelligent cleaning path planning for sanitation vehicles based on urban roads, the identification of road edges and obstacle contours in the working environment of the sanitation vehicle using a lidar sensor and a binocular camera sensor includes the following steps:

[0076] Step 201: Measure the time difference between the emission and reception of the laser pulse using a lidar sensor, and calculate the distance between the object and the sensor by combining the laser emission angle and velocity to obtain three-dimensional point cloud data;

[0077] Step 202: Process the 3D point cloud data and use clustering algorithms to distinguish road edges and obstacles;

[0078] Step 203: Use the binocular camera sensor to acquire image data, correct and match the images captured by the left and right cameras, and obtain the corresponding points of the same object in the left and right images; calculate the depth information of the object according to the parallax formula to obtain the three-dimensional coordinate data of the object.

[0079] Step 204: Process the three-dimensional coordinate data to identify the specific shapes and locations of road edges and obstacles, and obtain the initial environmental data of the sanitation vehicle.

[0080] Please see Figure 3 In a method for intelligent cleaning path planning of sanitation vehicles based on urban roads, the initial pose data of the sanitation vehicle is obtained by using extended Kalman filtering to fuse IMU data and correct positioning deviations, including the following steps:

[0081] Step 301: Based on the pose estimation of the previous moment and the acceleration and angular velocity measured by the IMU, use the kinematic model to predict the pose estimation value of the current moment, which includes at least position, velocity and attitude angle.

[0082] Step 302: Obtain the initial environmental data of the sanitation vehicle, match the initial environmental data of the sanitation vehicle with the pose estimation value, convert the pose estimation value into the observation value through the measurement model, calculate with the actual observation value, and obtain the observation residual;

[0083] Step 303: Using the gain matrix of the extended Kalman filter, the observation residuals are incorporated into the pose estimation value, the error covariance matrix is ​​updated, and the initial pose data of the sanitation vehicle is obtained.

[0084] Specifically, the present invention can also be implemented through the following steps:

[0085] I. Acquisition of Initial Pose Data

[0086] (I) Sensor Principles and Environmental Recognition

[0087] LiDAR sensor

[0088] LiDAR sensors construct environmental contours by emitting laser beams and receiving reflected signals. Specifically, an internal laser emitter emits laser pulses at a specific frequency and angle. These pulses are reflected when they encounter objects such as road edges or obstacles, and the reflected light is captured by a receiver. By measuring the time difference between the laser pulse's emission and reception, and combining this with the known laser emission angle and velocity, the distance between the object and the sensor can be calculated. As the sensor rotates or scans, it continuously acquires distance information about the surrounding environment, thereby constructing 3D point cloud data. This point cloud data is then processed, such as using clustering algorithms to distinguish road edges and obstacles, to identify their approximate contours.

[0089] Binocular camera sensor

[0090] A binocular camera sensor consists of two parallel cameras, simulating the visual principle of human eyes. A baseline distance exists between the two cameras. When capturing the same scene, the same object will appear in different positions in the images from the two cameras, a phenomenon known as parallax. By calculating the parallax and using triangulation principles, the three-dimensional coordinates of the object can be obtained. The specific steps are as follows: First, the images captured by the left and right cameras are calibrated and matched to find the corresponding points of the same object in the left and right images; then, the depth information of the object is calculated according to the parallax formula, thus obtaining the object's three-dimensional coordinates; finally, these three-dimensional coordinate data are processed to identify the specific shape and location of road edges and obstacles. For example, edge detection algorithms are used to extract feature points of road edges, and contour detection algorithms are used to identify the contours of obstacles.

[0091] (ii) Positioning deviation correction

[0092] IMUs (Inertial Measurement Units) provide data such as acceleration and angular velocity for sanitation vehicles. However, due to IMU drift errors, positioning errors accumulate over time. Extended Kalman filtering (EKF) is a filtering method suitable for nonlinear systems, used to fuse data from lidar, binocular cameras, and IMUs to correct positioning deviations.

[0093] The specific process is as follows:

[0094] Prediction steps: Based on the pose estimate from the previous moment and the acceleration and angular velocity measured by the IMU, the kinematic model is used to predict the pose estimate for the current moment, including position, velocity, and attitude angles. Simultaneously, the covariance matrix of the prediction error is calculated to describe the uncertainty of the prediction.

[0095] Update steps: The location information of road edges and obstacles identified by the LiDAR and binocular camera is matched with the predicted pose to obtain observation values. The predicted pose is converted into theoretical observation values ​​using a measurement model, and compared with actual observation values ​​to calculate the observation residuals. The observation residuals are then incorporated into the predicted pose using the gain matrix of the extended Kalman filter to obtain a more accurate pose estimate, and the error covariance matrix is ​​updated. By continuously repeating the prediction and update steps, the positioning deviation of the sanitation vehicle is gradually corrected, ultimately obtaining accurate initial pose data.

[0096] II. Initial Path Generation and Optimization

[0097] (a) Data Acquisition

[0098] High-precision maps are collected and produced by professional surveying companies using high-precision surveying equipment, such as drones and vehicle-mounted LiDAR. High-precision maps contain detailed road information, such as the number of lanes, road width, slope, curvature, road markings, and traffic sign locations. This information provides precise road geometry and constraints for route planning.

[0099] Dynamic traffic data can be obtained in two ways. On the one hand, it can be obtained from the real-time database of traffic management departments, such as traffic flow, congestion, traffic accident information, and the real-time status of traffic lights (remaining time for green, red, and yellow lights). On the other hand, it can be collected in real time by sensors installed on sanitation vehicles, such as vehicle-mounted cameras identifying the speed and position of surrounding vehicles, and vehicle-mounted radar detecting the distance and relative speed between vehicles and surrounding objects.

[0100] (II) Marking of Energy Consumption Hotspot Areas

[0101] In the road topology map, energy hotspot areas are marked based on the following factors:

[0102] Road gradient: On roads with steep gradients, sanitation vehicles need to overcome gravity to work uphill, increasing energy consumption; while they can utilize gravity when going downhill, frequent braking also leads to energy consumption. By obtaining gradient information for each road segment through high-precision maps, areas with gradients exceeding a certain threshold (5%) are marked as energy consumption hotspots.

[0103] Traffic flow: In areas with high traffic volume, sanitation vehicles need to frequently start, brake, and decelerate, leading to increased energy consumption of the engine and braking system. Based on dynamic traffic data, road sections with traffic flow exceeding a set threshold (200 vehicles per hour) are marked as energy consumption hotspots.

[0104] Road curvature: On curves with significant curvature, sanitation vehicles need to slow down, and the steering system consumes more energy. By combining road curvature information from high-precision maps, curve areas with a curvature radius of less than a certain value (50 meters) are marked as energy consumption hotspots.

[0105] (III) Initial Path Generation

[0106] The improved A algorithm, based on the traditional A algorithm, introduces an energy consumption factor as part of the heuristic function to generate a more energy-efficient initial path. The specific steps are as follows:

[0107] Define the state space: Define the nodes in the road topology graph as intersections or endpoints of road segments. The state of a node includes its position, direction, etc.

[0108] The cost function consists of two parts: the actual cost g(n) from the starting point to the current node and the heuristic cost h(n) from the current node to the destination. The actual cost g(n) considers not only the travel distance but also the weights of energy-intensive hotspots; for example, the cost increases when traveling in an energy-intensive hotspot. The heuristic cost h(n) uses an improved Manhattan distance or Euclidean distance and considers road capacity and energy consumption factors.

[0109] Search path: Nodes with the minimum cost function are selected through a priority queue and expanded until the destination node is found. During the search, obstacles and prohibited areas are avoided, while traffic light constraints are also considered.

[0110] (iv) Path optimization

[0111] Genetic algorithms are used to optimize Pareto front solutions to find the optimal balance among multiple objectives such as path length, energy consumption, and travel time. The specific steps are as follows:

[0112] Population initialization: Generate a set of initial paths as the population. Each path is represented as a chromosome, and the genes of the chromosome can be the sequence of nodes passed through by the path.

[0113] Fitness calculation: Define a fitness function that comprehensively considers multiple objectives such as path length, energy consumption, and travel time. For multi-objective optimization problems, Pareto dominance is used to evaluate the quality of individual solutions, and non-dominated solutions are retained as Pareto front solutions.

[0114] Genetic operations include selection, crossover, and mutation. Selection uses methods such as roulette wheel selection or tournament selection to choose individuals with high fitness as parents; crossover generates offspring chromosomes by exchanging parts of the genes on the parent chromosomes; mutation randomly modifies the genes on chromosomes, such as inserting, deleting, or replacing nodes, to increase population diversity.

[0115] Iterative optimization: After several generations of genetic operations, the population gradually approaches the Pareto front, and finally obtains the optimized first working path.

[0116] III. Generation of the Second Working Path

[0117] (I) LPV-MPC Control Model

[0118] The LPV (Linear Variable Parameter)-MPC (Model Predictive Control) control model is a model-based control method that can adjust the control strategy in real time according to changes in system parameters. During operation, the load on a sanitation vehicle changes as garbage is collected, and braking delay is affected by road conditions, vehicle status, and other factors, all of which lead to changes in system parameters. The LPV-MPC control model establishes a dynamic model of the sanitation vehicle, using parameters such as load and braking delay as time-varying parameters. Within each control cycle, based on current parameter estimates and future predicted inputs, it calculates the optimal control sequence, such as throttle, brake, and steering commands, to achieve precise vehicle control.

[0119] (ii) Smith predictor compensation

[0120] The Smith predictor is used to compensate for load changes and braking delays in sanitation vehicles during operation. Load changes affect the vehicle's dynamic characteristics, such as inertial mass and tire friction; braking delay is the lag in braking effect caused by the response time of the braking system and the vehicle's motion inertia. The Smith predictor establishes a predictive model of the system to predict the impact of load changes and braking delays on the system output and compensates for them in advance in the control input to reduce the deviation between the actual output and the expected output. Specifically, the actual system is connected in parallel with the predictive model, and the control input is adjusted by comparing the output difference between the two, thereby achieving compensation for load changes and braking delays. (III) Adaptive PID algorithm adjusts yaw angle

[0121] Adaptive PID algorithms automatically adjust the parameters of the PID controller based on the driving status of the sanitation vehicle to improve the accuracy and robustness of yaw angle control. Traditional PID controllers have fixed parameters, making it difficult to adapt to complex driving environments and changes in vehicle status. Adaptive PID algorithms monitor real-time information such as yaw angle error, speed, and road conditions, and use adaptive algorithms (fuzzy logic, neural networks, etc.) to adjust the proportional (P), integral (I), and derivative (D) parameters online, enabling the controller to better adapt to different operating conditions. For example, when the vehicle is driving on a curve, the proportional parameter is increased to quickly respond to yaw angle errors based on the curve's curvature and the vehicle's speed; when the vehicle is driving in a straight line, the integral parameter is decreased to avoid integral saturation.

[0122] (iv) Feedback of Path Tracking Error Model

[0123] The path tracking error model calculates the lateral and heading angle errors between the actual and desired paths and feeds this error information back to the MPC controller. The lateral error refers to the vertical distance between the vehicle's current position and the desired path, while the heading angle error is the angle between the vehicle's current heading angle and the tangent direction of the desired path. Based on this error information and future predicted inputs, the MPC controller optimizes the control sequence, adjusting the vehicle's direction and speed to ensure accurate path tracking. Through continuous feedback and optimization, a more precise second working path is obtained.

[0124] IV. Target Working Path Generation

[0125] (a) Green wave vehicle speed calculation module

[0126] The green wave speed calculation module calculates the appropriate speed for vehicles to pass through the intersection when the light turns green, based on the traffic light cycle, the current vehicle position, and speed. The specific steps are as follows:

[0127] Obtain traffic light information: Obtain the cycle duration, current signal status, and remaining time of the traffic light ahead through dynamic traffic data.

[0128] Calculate distance and time: Calculate the distance from the vehicle's current location to the traffic light intersection, and combine this with the remaining time of the traffic light to determine the traffic light status when the vehicle arrives at the intersection.

[0129] Determine the green wave speed: If a vehicle can reach the intersection within the green light period, calculate a constant speed range so that the vehicle can pass through the intersection when the green light turns on; if the current traffic light is red, calculate the remaining time of the red light and the time required for the vehicle to reach the intersection, and determine the appropriate speed at which the vehicle can pass through the intersection after the red light turns green. (II) Decision and Instruction Generation

[0130] When a vehicle approaches a traffic light, the green wave speed calculation module is activated:

[0131] If it is predicted that the vehicle can pass through the intersection when the light is green, a constant speed command is output to keep the vehicle at the calculated green wave speed so as to pass through the intersection smoothly.

[0132] Impassable situations: If it is predicted that the vehicle cannot pass through the intersection when the light is green, or that it may encounter consecutive red lights after passing through the intersection, resulting in frequent stops, a turning instruction is generated based on the surrounding road topology map and dynamic traffic data. The appropriate route is selected to bypass the intersection, such as choosing an adjacent parallel road, to avoid waiting at red light intersections, thereby improving the work efficiency and driving safety of sanitation vehicles.

[0133] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely preferred examples and are not intended to limit the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed. The scope of protection of the present invention is defined by the appended claims and their equivalents.

Claims

1. A method for intelligent sweeping path planning for sanitation vehicles based on urban roads, characterized in that, The intelligent cleaning path planning method for sanitation vehicles includes the following steps: The road edges and obstacle contours in the working environment of the sanitation vehicle are identified by LiDAR sensor and binocular camera sensor. The positioning deviation is corrected by using extended Kalman filter to fuse IMU data and obtain the initial pose data of the sanitation vehicle. High-precision maps and dynamic traffic data are acquired, energy consumption hotspots are marked on the road topology map, an initial path is generated using an improved A* algorithm, and then the Pareto front solution is optimized using a genetic algorithm to obtain the first working path of the sanitation vehicle. An LPV-MPC control model for a sanitation vehicle is established, with load and braking delay parameters as time-varying parameters. Based on the current parameter estimates and future predicted inputs, the optimal control sequence is calculated. The Smith predictor is used to compensate for load changes and braking delays during the operation of the sanitation vehicle, thus obtaining the second working path of the sanitation vehicle. The system obtains the cycle duration, current signal light status, and remaining time of the traffic lights ahead by using dynamic traffic data; calculates the distance from the vehicle's current location to the traffic light intersection, and, combined with the remaining time of the traffic lights, obtains the signal light status when the vehicle arrives at the intersection; and makes predictions based on the signal light status. If the prediction is successful, a constant speed driving instruction is output; otherwise, a turning instruction is generated to obtain the target working path of the sanitation vehicle.

2. The intelligent sweeping path planning method for sanitation vehicles based on urban roads as described in claim 1, characterized in that, The method of identifying road edges and obstacle outlines in the working environment of sanitation vehicles using lidar sensors and binocular camera sensors includes: By measuring the time difference between the emission and reception of a laser pulse using a lidar sensor, and combining the laser emission angle and velocity, the distance between the object and the sensor is calculated to obtain three-dimensional point cloud data. The three-dimensional point cloud data is processed, and a clustering algorithm is used to distinguish road edges and obstacles; Image data is acquired using a binocular camera sensor. The images captured by the left and right cameras are corrected and matched to obtain the corresponding points of the same object in the left and right images. The depth information of the object is calculated according to the parallax formula to obtain the three-dimensional coordinate data of the object. The three-dimensional coordinate data is processed to identify the specific shapes and locations of road edges and obstacles, thereby obtaining the initial environmental data of the sanitation vehicle.

3. The intelligent sweeping path planning method for sanitation vehicles based on urban roads as described in claim 1, characterized in that, The process of using extended Kalman filtering to fuse IMU data to correct positioning errors and obtain initial pose data for the sanitation vehicle includes: Based on the pose estimate from the previous moment and the acceleration and angular velocity measured by the IMU, the pose estimate for the current moment is predicted using a kinematic model, including at least position, velocity, and attitude angle. Acquire initial environmental data of the sanitation vehicle, match the initial environmental data of the sanitation vehicle with the pose estimation value, convert the pose estimation value into the observation value through the measurement model, calculate with the actual observation value to obtain the observation residual; By incorporating the observation residuals into the pose estimate using the gain matrix of the extended Kalman filter, the error covariance matrix is ​​updated to obtain the initial pose data of the sanitation vehicle.

4. The intelligent sweeping path planning method for sanitation vehicles based on urban roads as described in claim 1, characterized in that, The process of acquiring high-precision maps and dynamic traffic data, marking energy consumption hotspots in the road topology map, and generating an initial path using an improved A* algorithm includes: Acquire high-precision maps and dynamic traffic data, and generate a road topology map for sanitation vehicle operations based on the high-precision maps; Based on the dynamic traffic data, energy consumption hotspots are marked on the road topology map, including at least road slope, traffic flow, and road curvature. The nodes in the road topology graph are defined as the endpoints of road segments, and the state of the nodes includes at least position and orientation; The cost function is calculated, including the actual cost from the starting point to the current node and the heuristic cost from the current node to the destination. The node with the minimum cost function is selected through a priority queue for expansion, and the destination node is found to obtain the initial path.

5. The intelligent sweeping path planning method for sanitation vehicles based on urban roads as described in claim 1, characterized in that, The Pareto front solution is then optimized using a genetic algorithm to obtain the first working path of the sanitation vehicle, including: Generate an initial set of paths as a population. Each path is represented as a chromosome, and the genes of the chromosome are the sequence of nodes traversed by the path. Define a fitness function, combine path length, energy consumption and travel time, use Pareto dominance to evaluate the quality of individuals, and retain non-dominated solutions as Pareto front solutions; Individuals with high fitness were selected as parents, and the population was gradually brought closer to the Pareto front through genetic manipulation and iteration to obtain the first working path of the sanitation vehicle.

6. A system for implementing the intelligent sweeping path planning method for sanitation vehicles based on urban roads as described in claim 1, characterized in that, The system includes the following modules: The pose data acquisition module is used to identify the road edges and obstacle contours in the working environment of the sanitation vehicle through the lidar sensor and the binocular camera sensor, and to correct the positioning deviation by using extended Kalman filter to fuse IMU data to obtain the initial pose data of the sanitation vehicle. The work path planning module is used to acquire high-precision maps and dynamic traffic data, mark energy consumption hotspots in the road topology map, generate an initial path using an improved A* algorithm, and then optimize the Pareto front solution using a genetic algorithm to obtain the first work path of the sanitation vehicle. The working path optimization module is used to establish the LPV-MPC control model of the sanitation vehicle, and uses the load and braking delay parameters as time-varying parameters. Based on the current parameter estimates and future predicted inputs, the optimal control sequence is calculated, and the Smith predictor is used to compensate for the load changes and braking delay of the sanitation vehicle during operation to obtain the second working path of the sanitation vehicle. The sanitation vehicle avoidance module is used to obtain the cycle duration, current signal light status, and remaining time of the traffic light ahead through dynamic traffic data; calculate the distance from the vehicle's current position to the traffic light intersection; and, in combination with the remaining time of the traffic light, obtain the signal light status when the vehicle arrives at the intersection. Based on the traffic light status, a prediction is made. If the prediction is successful, a constant speed driving command is output; otherwise, a turning command is generated to obtain the target working path of the sanitation vehicle.

Images