Sanitation vehicle sweeping path planning method and system

Abstract

The invention relates to the technical field of path planning, and discloses a path planning method and system for sweeping of a sanitation truck, and the method comprises the steps: obtaining the real-time data of an obstacle; constructing an attraction and rejection combined constraint function for the close cleaning obstacles; solving to obtain a planned optimal speed and an optimal yaw angle; the path point density and speed are dynamically adjusted based on the target function; performing correction control on the yaw angle by adopting a PID algorithm; the technical problems that in the prior art, accurate control over the welt distance is insufficient, the road edge is regarded as a barrier needing to be away, and the welt cleaning requirement of a cleaning disc and the road edge cannot be met, so that a blind area is cleaned are solved. According to the method, by designing the attraction and rejection combined constraint function and combining target function optimization and a dynamic adjustment mechanism, accurate control over the welt distance is achieved, meanwhile, the safe distance between the vehicle and the obstacle is considered, and therefore the problems of blind area cleaning and potential safety hazards are avoided, and the accuracy and safety of path planning are improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of path planning, and in particular relates to a path planning method and system for cleaning by a sanitation vehicle. Background Art

[0002] At present, the path planning technology of unmanned sanitation vehicles in complex environments still has many shortcomings. For example, in the existing technology, the curb is usually treated as an obstacle that needs to be kept away, and it is impossible to achieve accurate edge cleaning, resulting in a cleaning blind spot between the cleaning disc and the curb; at the same time, in order to get close to the cleaning path, the existing method often weakens the "push away" constraint of the obstacle, which easily leads to insufficient safety distance between the vehicle and other dynamic obstacles such as pedestrians and vehicles, increasing the operation risk; in addition, in complex scenes such as irregular curbs or dynamic obstacles, the real-time and stability of path planning are difficult to balance, further limiting the improvement of cleaning efficiency; the existing technology cannot fully meet the requirements of unmanned sanitation vehicles for edge cleaning accuracy and safety. Therefore, there is an urgent need for a path planning method that can accurately control the edge distance between the vehicle and the curb in complex environments and dynamic scenes, while ensuring a safe distance from other obstacles, so as to achieve high accuracy and stability of path planning while ensuring cleaning efficiency, so as to improve the practicality and reliability of unmanned sanitation vehicles in cleaning tasks. Summary of the invention

[0003] In view of the above-mentioned technical deficiencies, the purpose of the present invention is to propose a path planning method for cleaning by sanitation vehicles, aiming to solve the technical problem of insufficient precise control of the edge distance in the prior art, treating the curb as an obstacle that needs to be kept away from, and failing to meet the edge cleaning requirements of the cleaning disc and the curb, resulting in a cleaning blind spot.

[0004] In order to solve the above technical problems, the present invention adopts the following technical solutions: The present invention provides a path planning method for cleaning by a sanitation vehicle.

[0005] The path planning method for cleaning by the sanitation vehicle comprises:

[0006] Step S10: Obtain real-time data of obstacles through the depth perception camera and laser radar on the sanitation vehicle env , real-time data of obstacles F env Obstacle information is classified into two categories, including close-to-clearance obstacles and detour obstacles.

[0007] Step S20: From the real-time data F of the obstacle env The position (x, y) of the sanitation vehicle in the plane, the yaw angle θ, the real-time edge distance between the sanitation vehicle and the curb, and the real-time distance d between the sanitation vehicle and the obstacle are obtained. obs, construct an attraction-repulsion joint constraint function f for close-to-clearance obstacles edge , the specific formula is:

[0008]

[0009] Among them, d min The minimum safe distance between the sanitation vehicle and the curb; d max The maximum allowable distance between the sanitation vehicle and the curb; d actual is the real-time edge distance between the sanitation vehicle and the curb; θ error is the current yaw angle error of the sanitation vehicle, which is determined according to the yaw angle θ and the preset yaw angle reference value; w repel 、w attract and w angle They are repulsion weight coefficient, attraction weight coefficient and yaw correction weight coefficient respectively;

[0010] Step S30: According to the attraction and repulsion joint constraint function f edge Design the path planning objective function f(B), and solve the path planning objective function f(B) to obtain the optimal planning speed v optimal and planning the optimal yaw angle e optimal ; Among them, the formula of the path planning objective function f(B) is:

[0011] f(B)=γ edge f edge +γ safe f safe +γ stability f stability

[0012] Among them, f safe is the safety constraint function; f stability is the stability constraint function; γ edge , γ safe and γ stability They are the edge constraint weight coefficient, safety constraint weight coefficient and stability constraint weight coefficient respectively;

[0013] Step S40: dynamically adjusting the sanitation vehicle path point density P and the sanitation vehicle speed v based on the objective function f(B);

[0014] Step S50: From the real-time data F of the obstacle env Get the real-time yaw angle, calculate the real-time yaw angle and the planned optimal yaw angle optimal The error e(t) is calculated and the PID algorithm is used to correct the yaw angle. The formula is:

[0015]

[0016] Wherein, Δe(t) is the yaw angle adjustment after correction; K p , K i and K d are proportional, integral and differential control parameters respectively; is the accumulation of yaw angle error; is the rate of change of the yaw angle error with time t.

[0017] Preferably, in step S30, the safety constraint f safe is based on the real-time distance d between the sanitation vehicle and the obstacle. obs The constraint is:

[0018]

[0019] Among them, d safe is the preset safety distance threshold between the sanitation vehicle and the obstacle, and N is the number of obstacle instances collected.

[0020] Preferably, in step S30, the stability constraint f stability The formula is:

[0021]

[0022] in is the target edge distance, and N is the number of obstacle instances collected.

[0023] Preferably, in step S40, the adjustment formula of the sanitation vehicle path point density P is:

[0024]

[0025] Among them, P actual is the adjusted path point density of the sanitation vehicle; Δp is the path point density adjustment increment; α and β are the control parameters of the path point density adjustment, which control the adjustment amplitude and convergence speed respectively.

[0026] Preferably, in step S40, the adjustment formula of the sanitation vehicle speed v is:

[0027]

[0028] Among them, v actual is the adjusted speed of the sanitation vehicle, is the velocity constraint f edge The impact of is the speed safety constraint f safe The influence of v Weight parameter adjusted for speed.

[0029] Preferably, in step S30, the optimal speed v is planned optimal The solution formula is:

[0030] v optimal =argmin v f(B)

[0031] Among them, argmin v f(B) represents the vehicle speed v that minimizes the function f(B).

[0032] Preferably, in step S30, the optimal yaw angle e is planned optimal The solution formula is:

[0033] e optimal =argmin θ (f edge +f stability )

[0034] Among them, argmin θ (f edge +f stability ) means to make f edge and stability cost f stability Find the angle θ that minimizes the sum.

[0035] The present invention also provides a path planning system for cleaning by a sanitation vehicle, comprising:

[0036] Obstacle data acquisition module, used to obtain real-time data of obstacles through the depth perception camera and lidar on the sanitation vehicle env , real-time data of obstacles F env Obstacle information is classified into two categories, including close-to-clearance obstacles and detour obstacles.

[0037] Path constraint building block for real-time data from obstacles F env The position (x, y) of the sanitation vehicle in the plane, the yaw angle θ, the real-time edge distance between the sanitation vehicle and the curb, and the real-time distance d between the sanitation vehicle and the obstacle are obtained. obs , construct an attraction-repulsion joint constraint function f for close-to-clearance obstacles edge , the specific formula is:

[0038]

[0039] Among them, d min The minimum safe distance between the sanitation vehicle and the curb; d max The maximum allowable distance between the sanitation vehicle and the curb; d actual is the real-time edge distance between the sanitation vehicle and the curb; θ error is the current yaw angle error of the sanitation vehicle, which is determined according to the yaw angle θ and the preset yaw angle reference value; w repel 、wattract and w angle They are repulsion weight coefficient, attraction weight coefficient and yaw correction weight coefficient respectively;

[0040] The path planning design module is used to combine the attraction and repulsion constraints function f edge Design the path planning objective function f(B), and solve the path planning objective function f(B) to obtain the optimal planning speed v optimal and planning the optimal yaw angle e optimal ; Among them, the formula of the path planning objective function f(B) is:

[0041] f(B)=γ edge f edge +γ safe f safe +γ stability f stability

[0042] Among them, f safe is the safety constraint function; f stability is the stability constraint function; γ edge , γ safe and γ stability They are the edge constraint weight coefficient, safety constraint weight coefficient and stability constraint weight coefficient respectively;

[0043] The speed dynamic adjustment module is used to dynamically adjust the sanitation vehicle path point density P and the sanitation vehicle speed v based on the objective function f(B);

[0044] Yaw angle correction module, used to obtain real-time data from obstacles F env Get the real-time yaw angle, calculate the real-time yaw angle and the planned optimal yaw angle optimal The error e(t) is calculated and the PID algorithm is used to correct the yaw angle. The formula is:

[0045]

[0046] Wherein, Δe(t) is the yaw angle adjustment after correction; K p , K i and K d are proportional, integral and differential control parameters respectively; is the accumulation of yaw angle error; is the rate of change of the yaw angle error with time t.

[0047] The present invention also provides a path planning device for cleaning by a sanitation vehicle, comprising a memory, a processor, and a path planning program for cleaning by a sanitation vehicle stored in the memory and executable on the processor. When the path planning program for cleaning by the sanitation vehicle is executed by the processor, the path planning method for cleaning by the sanitation vehicle is implemented.

[0048] The present invention also provides a computer program product, including a path planning program for cleaning by a sanitation vehicle, wherein the path planning program for cleaning by a sanitation vehicle implements the path planning method for cleaning by a sanitation vehicle when the program is executed by a processor.

[0049] The beneficial effect of the present invention is that compared with the prior art, the precise control of the edge distance is insufficient, the curb is regarded as an obstacle that needs to be kept away, and the requirement of edge cleaning between the cleaning disc and the curb cannot be met, resulting in the technical problem of cleaning blind spots. The present application achieves precise control of the edge distance by designing an attraction-repulsion joint constraint function, combining objective function optimization and dynamic adjustment mechanism, while taking into account the safe distance between the vehicle and the obstacle, thereby avoiding the problem of cleaning blind spots and safety hazards, and improving the accuracy and safety of path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0051] Figure 1 The figure is a flow chart of a first embodiment of a path planning method for cleaning by a sanitation vehicle according to the present invention.

[0052] Figure 2 The present invention is a schematic diagram of equipment for a path planning method for cleaning by a sanitation vehicle. DETAILED DESCRIPTION

[0053] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0054] Embodiment 1: Figure 1 , which is a flow chart of a first embodiment of a path planning method for cleaning by a sanitation vehicle of the present invention, and provides a first embodiment of a path planning method for cleaning by a sanitation vehicle of the present invention.

[0055] In the first embodiment, the path planning method for cleaning by the sanitation vehicle includes:

[0056] Step S10: Obtain real-time data of obstacles through the depth perception camera and laser radar on the sanitation vehicle env , real-time data of obstacles F env The obstacle information is classified into two categories, including close-to-clearance obstacles and detour obstacles.

[0057] It should be noted that the depth perception camera is used to obtain the three-dimensional structural information of obstacles and provide accurate spatial position and shape data; the lidar is used to generate high-precision point cloud data, which can detect the distance, size and relative speed information of obstacles in real time; the data of the depth perception camera and the lidar are fused to ensure the accuracy and real-time performance of obstacle detection.

[0058] It is understandable that the main basis for classification is the type and location of obstacles, such as curbs and border plants, which are usually static objects and are located at the edge of the cleaning path and need to be cleaned closely; dynamic objects such as pedestrians and vehicles have greater uncertainty and need to be bypassed to avoid collisions; data classification is achieved through machine learning models, using pre-trained deep learning models to perform real-time classification of point cloud data and image data.

[0059] Step S20: From the real-time data F of the obstacle env The position (x, y) of the sanitation vehicle in the plane, the yaw angle θ, the real-time edge distance between the sanitation vehicle and the curb, and the real-time distance d between the sanitation vehicle and the obstacle are obtained. obs , construct an attraction-repulsion joint constraint function f for close-to-clearance obstacles edge , the specific formula is:

[0060]

[0061] Among them, d min The minimum safe distance between the sanitation vehicle and the curb; d max The maximum allowable distance between the sanitation vehicle and the curb; d actual is the real-time edge distance between the sanitation vehicle and the curb; θ error is the current yaw angle error of the sanitation vehicle, which is determined according to the yaw angle θ and the preset yaw angle reference value; w repel 、w attract and w angle They are repulsion weight coefficient, attraction weight coefficient and yaw correction weight coefficient respectively;

[0062] It should be noted that d minThe minimum safe distance between the sanitation vehicle and the curb, which is used to prevent the vehicle from getting too close to the curb, which may cause damage to the vehicle hardware or unnecessary friction; max The maximum allowable distance between the sanitation vehicle and the curb, ensuring that the sweeping disc can fully cover the curb area; d actual The real-time distance between the sanitation vehicle and the curb is dynamically updated and participates in the calculation in real time; θ error is the current yaw angle error of the sanitation vehicle, defined as θ target -θ actual , used to correct the vehicle direction deviation; when d actual <d min When the exclusion term w repel Enhanced, forcing vehicles to stay away from the curb and avoid getting too close; when d actual >d rmax When the attraction term w attract Enhance, pull the vehicle closer to the curb to ensure sweeping coverage; when d min ≤d max When , only the yaw correction term w angle ·|θ error |Works to ensure a smooth path.

[0063] For example, in a cleaning task, the sanitation vehicle detects the real-time distance d from the roadside. actual =0.3m, minimum safe edge distance d min =0.2m, maximum allowable edge distance d max =0.4m; if d actual =0.15m, exceeding the minimum safety distance, exclusion term w repek will be activated, and the constraint function outputs a larger value, forcing the vehicle to stay away from the curb; if d actual =0.35m, which is within the normal range, with only the yaw correction item w angle ·|θ error |Function to maintain the vehicle's directional stability.

[0064] Step S30: According to the attraction and repulsion joint constraint function f edge Design the path planning objective function f(B), and solve the path planning objective function f(B) to obtain the optimal planning speed v optimal and planning the optimal yaw angle e optimal ; Among them, the formula of the path planning objective function f(B) is:

[0065] f(B)=γ edge f edge +γ safe f safe +γ stability f stability

[0066] Among them, f safe is the safety constraint function; f stability is the stability constraint function; γ edge , γ safe and γ stability They are the edge constraint weight coefficient, safety constraint weight coefficient and stability constraint weight coefficient respectively;

[0067] It should be noted that f(B) is the optimization goal of the entire path planning, which comprehensively considers the edge cleaning accuracy, safety and path stability, and is optimized by the weight coefficient γ edge , γ safe and γ stability Dynamically balance the priorities of different constraints; f safe Ensure that the distance between the vehicle and the curb meets the attraction and repulsion constraints; stability Constrain the safe distance between vehicles and obstacles to avoid collision; stability Control the smoothness of the path and reduce the fluctuation of the edge distance.

[0068] For example, in a cleaning task, the sanitation vehicle needs to clean along a curved curb, and the distance d from the curb to the curb is actual Dynamic changes, system priority optimization edge , to ensure the edge accuracy; a pedestrian is detected near the curb, and the system increases f in real time safe The weight of the vehicle forces the vehicle to slow down and adjust the yaw angle to avoid collision. The system optimizes the objective function f(B) comprehensively and finally outputs the optimal speed v optimal =1.2m / s and optimal yaw angle e optimal =3°.

[0069] Step S40: dynamically adjusting the sanitation vehicle path point density P and the sanitation vehicle speed v based on the objective function f(B);

[0070] It should be noted that in step S40, the adjustment formula of the sanitation vehicle path point density P is:

[0071]

[0072] Among them, P actual is the adjusted path point density of the sanitation vehicle; ΔP is the path point density adjustment increment; α and β are the control parameters of the path point density adjustment, which control the adjustment amplitude and convergence speed respectively.

[0073] The adjustment formula of the sanitation vehicle speed v is:

[0074]

[0075] in, is the velocity constraint fedge The impact of is the speed safety constraint f safe The influence of v Weight parameter adjusted for speed

[0076] It should be understood that the adjustment of the waypoint density affects the precision of the path planning, while the speed adjustment affects the dynamic performance of the vehicle. The two need to be coordinated and optimized in combination with the actual scene. For example, in complex scenes, the increase of the waypoint density should be accompanied by an appropriate reduction of the vehicle speed to ensure that the vehicle can follow the path safely and smoothly. Parameters α and β control the range and sensitivity of the waypoint density adjustment and need to be adjusted according to the complexity of the scene. Parameter δ v Determines the sensitivity of speed adjustment. A larger value should be taken when there are many dynamic obstacles to give priority to driving safety.

[0077] For example, in a certain cleaning scenario, the constraint result of the path planning objective function is f edge =0.5, f safe =0.8, f stability =0.3; weight coefficient is γ edge =1.0,γ safe =1.5,γ stability =0.8, then the path point density adjustment is If v optimal =1.5m / s, then the actual adjusted speed is After adjustment, the speed may be reduced to 1.2m / s to meet the safety requirements of path planning.

[0078] Step S50: From the real-time data F of the obstacle env Get the real-time yaw angle, calculate the real-time yaw angle and the planned optimal yaw angle optimal The error e(t) is calculated and the PID algorithm is used to correct the yaw angle. The formula is:

[0079]

[0080] Wherein, Δe(t) is the yaw angle adjustment after correction; K p , K i and K d are proportional, integral and differential control parameters respectively; is the accumulation of yaw angle error; is the rate of change of the yaw angle error with time t.

[0081] It should be understood that the PID controller can adjust the yaw angle in real time to ensure that the vehicle always travels along the planned path. Through appropriate parameter adjustment, path tracking accuracy and vehicle stability can be taken into account in a dynamic environment.

[0082] In addition, the path planning system for cleaning by a sanitation vehicle provided by the present invention adopts a path planning method for cleaning by a sanitation vehicle in the above embodiment, which can solve the technical problem of path planning for cleaning by a sanitation vehicle. Compared with the prior art, the beneficial effects of the path planning system for cleaning by a sanitation vehicle provided by the present invention are the same as the beneficial effects of the path planning method for cleaning by a sanitation vehicle provided by the above embodiment, and the other technical features of the path planning system for cleaning by a sanitation vehicle are the same as the features disclosed in the above embodiment method, which will not be repeated here.

[0083] The present invention provides a path planning device for cleaning by a sanitation vehicle, please refer to Figure 2, a path planning device for cleaning by a sanitation vehicle comprises: at least one processor; and a memory connected in communication with the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute a path planning method for cleaning by a sanitation vehicle in the above-mentioned embodiment 1. A path planning device for cleaning by a sanitation vehicle in an embodiment of the present invention may include but is not limited to mobile terminals such as mobile phones, laptop computers, digital broadcast receivers, PDAs (Personal Digital Assistants), PADs (Portable Application Descriptions), PMPs (Portable Media Players), vehicle-mounted terminals (such as vehicle-mounted navigation terminals), etc., and fixed terminals such as digital TVs, desktop computers, etc. A path planning device for cleaning by a sanitation vehicle is merely an example and should not impose any limitations on the functions and scope of use of the embodiments of the present invention. A path planning device for cleaning by a sanitation vehicle may include a processing device 1001 (e.g., a central processing unit, a graphics processing unit, etc.), which can perform various appropriate actions and processes according to a program stored in a read-only memory (ROM: Read Only Memory) 1002 or a program loaded from a storage device 1003 to a random access memory (RAM: Random Access Memory) 1004. In RAM1004, various programs and data required for the operation of a path planning device for cleaning by a sanitation vehicle are also stored. The processing device 1001, ROM1002, and RAM1004 are connected to each other via a bus 1005. An input / output (I / O) interface 1006 is also connected to the bus. Typically, the following systems can be connected to the I / O interface 1006: input devices 1007 including, for example, a touch screen, a touchpad, a keyboard, a mouse, an image sensor, a microphone, an accelerometer, a gyroscope, etc.; output devices 1008 including, for example, a liquid crystal display (LCD), a speaker, a vibrator, etc.; storage devices 1003 including, for example, a magnetic tape, a hard disk, etc.; and communication devices 1009. The communication device 1009 can allow a path planning device for cleaning by a sanitation vehicle to communicate wirelessly or wired with other devices to exchange data. Although a path planning device for cleaning by a sanitation vehicle with various systems is shown in the figure, it should be understood that it is not required to implement or have all the systems shown. More or fewer systems may be implemented or have alternatively.

[0084] The present invention also provides a computer program product, including a computer program, which implements the steps of the above-mentioned path planning method for cleaning by a sanitation vehicle when executed by a processor. The computer program product provided by the present invention can solve the technical problem of path planning for cleaning by a sanitation vehicle. Compared with the prior art, the beneficial effects of the computer program product provided by the present invention are the same as the beneficial effects of the path planning method for cleaning by a sanitation vehicle provided in the above-mentioned embodiment, which will not be described in detail here.

[0085] In particular, according to the embodiments disclosed in the present invention, the process described above with reference to the flowchart can be implemented as a computer software program. For example, the embodiments disclosed in the present invention include a computer program product, which includes a computer program carried on a computer-readable medium, and the computer program contains program code for executing the method shown in the flowchart. In such an embodiment, the computer program can be downloaded and installed from the network through a communication device, or installed from the storage device 1003, or installed from the ROM 1002. When the computer program is executed by the processing device 1001, the above-mentioned functions defined in the method of the embodiment disclosed in the present invention are executed.

[0086] It should be understood that the various parts disclosed in the present invention can be implemented by hardware, software, firmware or a combination thereof. In the description of the above embodiments, specific features, structures, materials or characteristics can be combined in any one or more embodiments or examples in a suitable manner.

[0087] Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.

Claims

1. A path planning method for cleaning by a sanitation vehicle, characterized in that: Methods include: Step S10: Obtain real-time data of obstacles through the depth perception camera and laser radar on the sanitation vehicle env , real-time data of obstacles F env Obstacle information is classified into two categories, including close-to-clearance obstacles and detour obstacles. Step S20: From the real-time data F of the obstacle env The position (x, y) of the sanitation vehicle in the plane, the yaw angle θ, the real-time edge distance between the sanitation vehicle and the curb, and the real-time distance d between the sanitation vehicle and the obstacle are obtained. obs , construct an attraction-repulsion joint constraint function f for close-to-clearance obstacles edge , the specific formula is: Among them, d min The minimum safe distance between the sanitation vehicle and the curb; d max The maximum allowable distance between the sanitation vehicle and the curb; d actual is the real-time edge distance between the sanitation vehicle and the curb; θ error is the current yaw angle error of the sanitation vehicle, which is determined according to the yaw angle θ and the preset yaw angle reference value; w repel 、w attract and w angle They are repulsion weight coefficient, attraction weight coefficient and yaw correction weight coefficient respectively; Step S30: According to the attraction-repulsion joint constraint function f edge Design the path planning objective function f(B), and solve the path planning objective function f(B) to obtain the optimal planning speed v optimal and planning the optimal yaw angle e optimal ; Among them, the formula of the path planning objective function f(B) is: f(B)=γ edge f edge +g safe f safe +g stability f stability Among them, f safe is the safety constraint function; f stability is the stability constraint function; γ edge , γ safe and γ stability They are the edge constraint weight coefficient, safety constraint weight coefficient and stability constraint weight coefficient respectively; Step S40: dynamically adjusting the sanitation vehicle path point density P and the sanitation vehicle speed v based on the objective function f(B) to obtain an adjusted sanitation vehicle path point density and an adjusted sanitation vehicle speed; Step S50: From the real-time data F of the obstacle env Get the real-time yaw angle, calculate the real-time yaw angle and the planned optimal yaw angle optimal The error e(t) is calculated and the PID algorithm is used to correct the yaw angle. The formula is: Wherein, Δe(t) is the yaw angle adjustment after correction; K p , K i and K d are proportional, integral and differential control parameters respectively; is the accumulation of yaw angle error; is the rate of change of the yaw angle error with time t.

2. A path planning method for cleaning by a sanitation vehicle as claimed in claim 1, characterized in that: In step S30, the safety constraint f safe is based on the real-time distance d between the sanitation vehicle and the obstacle. obs The constraint is: Among them, d safe is the preset safety distance threshold between the sanitation vehicle and the obstacle, and N is the number of obstacle instances collected.

3. A path planning method for cleaning by a sanitation vehicle as claimed in claim 1, characterized in that: In step S30, the stability constraint f stability The formula is: in is the target edge distance, and N is the number of obstacle instances collected.

4. A path planning method for cleaning by a sanitation vehicle as claimed in claim 1, characterized in that: In step S40, the adjustment formula of the sanitation vehicle path point density P is: Among them, P actual is the adjusted path point density of the sanitation vehicle; ΔP is the path point density adjustment increment; α and β are the control parameters of the path point density adjustment, which control the adjustment amplitude and convergence speed respectively.

5. A path planning method for cleaning by a sanitation vehicle as claimed in claim 1, characterized in that: In step S40, the adjustment formula of the sanitation vehicle speed v is: Among them, v actual is the adjusted speed of the sanitation vehicle, is the velocity constraint f edge The impact of is the speed safety constraint f safe The influence of v Weight parameter adjusted for speed.

6. A path planning method for cleaning by a sanitation vehicle as claimed in claim 1, characterized in that: In step S30, the optimal speed v is planned optimal The solution formula is: v optimal =argmin v f(B) Among them, argmin v f(B) represents the vehicle speed v that minimizes the function f(B).

7. A path planning method for cleaning by a sanitation vehicle as claimed in claim 1, characterized in that: In step S30, the optimal yaw angle e is planned optimal The solution formula is: e optimal =argmin θ (f edge +f stability ) Among them, argmin θ (f edge +f stability ) means to make f edge and stability cost f stability Find the angle θ that minimizes the sum.

8. A path planning system for cleaning by a sanitation vehicle, characterized in that: The path planning system for cleaning by the sanitation vehicle includes: The obstacle data acquisition module is used to obtain real-time data of obstacles through the depth perception camera and lidar on the sanitation vehicle. env , real-time data of obstacles F env Obstacle information is classified into two categories, including close-to-clearance obstacles and detour obstacles. Path constraint building block for real-time data from obstacles F env The position (x, y) of the sanitation vehicle in the plane, the yaw angle θ, the real-time edge distance between the sanitation vehicle and the curb, and the real-time distance d between the sanitation vehicle and the obstacle are obtained. obs , construct an attraction-repulsion joint constraint function f for close-to-clearance obstacles edge , the specific formula is: Among them, d min The minimum safe distance between the sanitation vehicle and the curb; d max The maximum allowable distance between the sanitation vehicle and the curb; d actual is the real-time edge distance between the sanitation vehicle and the curb; θ error is the current yaw angle error of the sanitation vehicle, which is determined according to the yaw angle θ and the preset yaw angle reference value; w repel 、w attract and w angle They are repulsion weight coefficient, attraction weight coefficient and yaw correction weight coefficient respectively; The path planning design module is used to combine the attraction and repulsion constraints function f edge Design the path planning objective function f(B), and solve the path planning objective function f(B) to obtain the optimal planning speed v optimal and planning the optimal yaw angle e optimal ; Among them, the formula of the path planning objective function f(B) is: f(B)=γ edge f edge +g safe f safe +g stability f stability Among them, f safe is the safety constraint function; f stability is the stability constraint function; γ edge , γ safe and γ stability They are the edge constraint weight coefficient, safety constraint weight coefficient and stability constraint weight coefficient respectively; The speed dynamic adjustment module is used to dynamically adjust the sanitation vehicle path point density P and the sanitation vehicle speed v based on the objective function f(B); Yaw angle correction module, used to obtain real-time data from obstacles F env Get the real-time yaw angle, calculate the real-time yaw angle and the planned optimal yaw angle optimal The error e(t) is calculated and the PID algorithm is used to correct the yaw angle. The formula is: Wherein, Δe(t) is the yaw angle adjustment after correction; K p , K i and K d are proportional, integral and differential control parameters respectively; is the accumulation of yaw angle error; is the rate of change of the yaw angle error with time t.

9. A path planning device for cleaning by a sanitation vehicle, characterized in that: The path planning device for cleaning by a sanitation vehicle comprises: a memory, a processor and a path planning program for cleaning by a sanitation vehicle stored in the memory and executable on the processor. When the path planning program for cleaning by a sanitation vehicle is executed by the processor, the path planning method for cleaning by a sanitation vehicle described in any one of claims 1 to 7 is implemented.

10. A computer program product, characterized in that The computer program product includes a path planning program for cleaning by a sanitation vehicle, and when the path planning program for cleaning by a sanitation vehicle is executed by a processor, the path planning method for cleaning by a sanitation vehicle described in any one of claims 1 to 7 is implemented.

Images