Unmanned cleaning vehicle path planning method based on dynamic bias, cleaning vehicle and medium

Abstract

The present application relates to unmanned cleaning vehicle navigation and control technology field, disclose a kind of unmanned cleaning vehicle path planning method based on dynamic bias, cleaning vehicle and medium.The method obtains the relative coordinates of the current position of unmanned cleaning vehicle and final target point, calculates ideal preview point, obtains the obstacle distribution information of surrounding environment by airborne laser radar.Environmental feature extraction model based on artificial potential field is constructed, target attractive force vector and obstacle repulsive force vector are calculated as physical feature vector.SAC-based decision network is constructed, physical feature vector and cleaning vehicle's own motion state are taken as state space, two-dimensional position bias quantity for ideal preview point is output to vector correct ideal preview point, generate dynamic virtual sub-target point and take this point as local navigation target, velocity sampling and trajectory evaluation are carried out using dynamic window method, and the optimal linear velocity and angular velocity control unmanned cleaning vehicle movement.The present application realizes that unmanned cleaning vehicle stably initiatively avoids obstacle.

Description

Technical Field

[0001] This invention relates to the field of navigation and control technology for unmanned cleaning vehicles, specifically a path planning method for unmanned cleaning vehicles based on dynamic bias, as well as an unmanned cleaning vehicle using this method and a computer-readable storage medium. Background Technology

[0002] With the rapid development of unmanned cleaning vehicle technology, automated unmanned cleaning vehicles are widely used in the holds of large cargo ships. Path planning for these vehicles is a key technology for achieving autonomous navigation. Currently, the Dynamic Window (DWA) method is widely used for local obstacle avoidance due to its low computational cost and compliance with the kinematic constraints of the cleaning vehicle. However, traditional DWA algorithms employ a greedy strategy, and the evaluation function tends to guide the cleaning vehicle directly towards the target point. When U-shaped walls, dead ends, or dense dynamic obstacles exist in the environment, the cleaning vehicle is prone to getting trapped in local minima and unable to escape.

[0003] While Deep Reinforcement Learning (DRL) methods possess global exploration capabilities, directly using high-dimensional LiDAR data as input leads to an excessively large state space, making training convergence difficult. Furthermore, the end-to-end output speed control commands often exhibit jitter and lack smoothness. Existing fusion methods mostly employ RL to dynamically adjust the weight coefficients of DWA, but this approach does not change the short-sighted nature of DWA, which "only looks at the destination," and still struggles to plan reasonable detour paths in complex terrain.

[0004] Therefore, how to develop a path planning method that can both escape local optima and ensure stable motion is a problem that urgently needs to be solved. Summary of the Invention

[0005] To address the technical problems existing in the prior art, this invention provides a path planning method for unmanned cleaning vehicles based on dynamic bias, a cleaning vehicle, and a medium. By extracting the physical vector features of the artificial potential field, the invention accelerates reinforcement learning training and uses a decision network based on SAC to output a virtual target bias to guide DWA, thereby enabling the unmanned cleaning vehicle to stably and actively avoid obstacles.

[0006] To achieve the above objectives, the present invention provides the following technical solution:

[0007] This invention discloses a path planning method for unmanned cleaning vehicles based on dynamic bias, comprising the following steps, namely S1 to S5.

[0008] S1. Obtain the relative coordinates of the current position and the final target point of the unmanned cleaning vehicle, construct a line-of-sight guidance vector from the current position to the final target, and calculate the ideal pre-aiming point position at a preset forward-looking distance along the direction of the vector. At the same time, obtain the obstacle distribution information of the surrounding environment in real time through the airborne lidar.

[0009] S2. Construct an environmental feature extraction model based on an artificial potential field. Based on the obstacle distribution information, use the environmental feature extraction model to calculate the target gravity vector and the resultant repulsion vector of the obstacles, and use the gravity vector and the resultant repulsion vector as physical feature vectors.

[0010] S3. Construct a decision network based on SAC, and input the physical feature vector and the motion state of the unmanned cleaning vehicle as the state space into the decision network, and output a two-dimensional position bias for the ideal aiming point.

[0011] S4. Using the two-dimensional position offset to perform vector correction on the ideal pre-aiming point to generate a dynamic virtual sub-target point, specifically including: transforming the two-dimensional position offset from the local coordinate system of the unmanned cleaning vehicle to the global coordinate system to obtain a global offset vector; calculating the magnitude of the resultant repulsive force vector of the obstacle, and using the hyperbolic tangent function to map the magnitude to obtain a nonlinear adjustment coefficient; using the nonlinear adjustment coefficient to weight the global offset vector, and superimposing the weighted vector onto the position of the ideal pre-aiming point to obtain a dynamic virtual sub-target point.

[0012] S5. Using dynamic virtual sub-target points as local navigation targets, the dynamic window method is used for velocity sampling and trajectory evaluation, and the optimal linear velocity and angular velocity are output to control the movement of the unmanned cleaning vehicle.

[0013] As a further improvement to the above scheme, in step S2, the calculation process of the environmental feature extraction model includes S21 to S24.

[0014] S21. Establish the gravitational potential field function and calculate the target gravitational vector pointing from the current position to the final target point.

[0015] S22. Establish the repulsive potential field function, and calculate the repulsive vector generated by each obstacle point within the sensing range of the airborne lidar.

[0016] S23. Superimpose the repulsive force vectors generated by all obstacles within the sensing range to obtain the resultant repulsive force vector of the obstacles.

[0017] S24. Normalize the target gravity vector and the combined repulsion vector of the obstacle to construct a physical feature vector.

[0018] As a further improvement to the above scheme, in step S3, the state space is defined as:

[0019] ;

[0020] In the formula, Represents the state space at the current time t; The current linear speed of the unmanned cleaning vehicle; The current angular velocity of the unmanned cleaning vehicle; The relative distance between the current position of the unmanned cleaning vehicle and the ideal aiming point; The relative angle between the current position of the unmanned cleaning vehicle and the ideal aiming point; , These are the components of the gravitational vector in the X and Y dimensions, respectively; , These are the components of the resultant repulsive force vector in the X and Y dimensions, respectively; superscript This is the transpose symbol.

[0021] As a further improvement to the above scheme, in step S3, the action space of the decision network is defined as the following continuous action vectors:

[0022] ;

[0023] In the formula, The action space at the current time t is the two-dimensional position offset relative to the ideal aiming point; and These represent the lateral and longitudinal coordinate offsets from the ideal pre-aiming point, respectively, within the coordinate system of the unmanned cleaning vehicle; the output layer of the decision network uses the Tanh activation function to restrict the actions to a preset range. Inner; superscript This is the transpose symbol.

[0024] As a further improvement to the above scheme, in step S4, the position expression of the dynamic virtual sub-target point is as follows:

[0025] ;

[0026] In the formula, This represents the position of the dynamically generated virtual sub-target point in the global coordinate system at the current moment. Let be the position of the ideal aiming point at the current moment; tanh(·) is the hyperbolic tangent function; The preset sensitivity coefficient; , These are the components of the resultant repulsive force vector in the X and Y dimensions, respectively; Let be the heading angle of the unmanned cleaning vehicle at the current time t; This is a rotation matrix used to transform the offset of the unmanned cleaning vehicle from the local coordinate system to the global coordinate system. The two-dimensional position offset is given.

[0027] As a further improvement to the above scheme, in step S5, the evaluation function of the dynamic window method is:

[0028] ;

[0029] In the formula, To achieve the current sampling rate The overall evaluation score is as follows: and These are the linear velocity and angular velocity of the unmanned cleaning vehicle, respectively. This is an azimuth evaluation function used to measure the orientation of the simulated trajectory's end point relative to the dynamic virtual sub-target point. The deviation between the two is derived from the kinematic model of the unmanned cleaning vehicle; This is a distance evaluation function used to measure the distance between the end of the simulated trajectory and the nearest obstacle; For velocity evaluation function; , , These are the corresponding weighting coefficients.

[0030] Among them, the sampling speed with the highest overall evaluation score was selected. The command is sent to the underlying driver of the unmanned cleaning vehicle for execution.

[0031] As a further improvement to the above scheme, the formula for calculating the azimuth evaluation function is as follows:

[0032] ;

[0033] In the formula, To achieve the current sampling rate Below, the predicted heading angle of the unmanned cleaning vehicle at the end of its trajectory is obtained through kinematic simulation. Pi is used to normalize angular deviations; The current location of the unmanned cleaning vehicle is pointed to a dynamic virtual sub-target point. The target azimuth angle is calculated using the following formula:

[0034] ;

[0035] In the formula, A function to calculate the arctangent value between two variables; and These are the x and y coordinates of the dynamic virtual sub-target point in the global coordinate system, respectively. and These are the x and y coordinates of the unmanned cleaning vehicle in the global coordinate system at the current time t;

[0036] The formula for calculating the distance evaluation function is:

[0037] ;

[0038] In the formula, Indicates the current sampling rate The minimum Euclidean distance between all points on the simulated trajectory and the nearest obstacle detected by the airborne lidar; This is to preset the safety evaluation threshold or the effective detection range of the airborne lidar;

[0039] The formula for calculating the velocity evaluation function is:

[0040] ;

[0041] In the formula, Candidate values ​​for linear velocity in the current sampling group; This refers to the maximum permissible linear speed of the unmanned cleaning vehicle under system settings or physical limitations.

[0042] As a further improvement to the above scheme, in step S3, the training reward function of the decision network includes the following parts:

[0043] Target proximity reward: Positively correlated with the change in distance of the unmanned cleaning vehicle as it approaches the final target point;

[0044] Collision penalty: A negative reward is given when the distance between the unmanned cleaning vehicle and an obstacle is less than the safe radius;

[0045] Bias penalty : In the formula, and These are the lateral and longitudinal coordinate offsets from the ideal aiming point, respectively, within the coordinate system of the unmanned cleaning vehicle itself. This is the penalty coefficient.

[0046] The present invention also discloses an unmanned cleaning vehicle, including a cleaning vehicle body and a control module mounted on the cleaning vehicle body. The control module includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the steps of the unmanned cleaning vehicle path planning method based on dynamic bias as described above.

[0047] The present invention also discloses a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the path planning method for unmanned cleaning vehicles based on dynamic bias as described above.

[0048] Compared with the prior art, the beneficial effects of the present invention are:

[0049] This invention uses the resultant force vector of the artificial potential field as the state input of the SAC (Support Algorithm), which greatly reduces the dimensionality of the state space compared to directly inputting the original radar point cloud, while preserving the physical topological features of the obstacle distribution, significantly improving the training speed and convergence stability of reinforcement learning. By generating virtual sub-targets through the position bias output of the SAC, it is equivalent to artificially creating a "guide point" in front of the DWA (Device Assisted Learning). When the unmanned cleaning vehicle encounters an obstacle, the SAC moves the virtual target to the side of the obstacle, inducing the DWA to perform a detour, thus solving the problem of the DWA easily getting trapped in local minima. Furthermore, the underlying control of the unmanned cleaning vehicle is still executed by the DWA that conforms to kinematic constraints, avoiding motion jitter caused by pure RL (Reinforcement Learning) control. Simultaneously, the bias output of the SAC only intervenes in areas with dense obstacles, automatically converging to zero in open areas through a penalty term, ensuring the continuity and stability of the cleaning task. Attached Figure Description

[0050] Figure 1 This is a flowchart of the unmanned cleaning vehicle path planning method based on dynamic bias in Embodiment 1 of the present invention.

[0051] Figure 2 This is a flowchart illustrating the overall architecture of the unmanned cleaning vehicle path planning method based on dynamic bias in Embodiment 1 of the present invention.

[0052] Figure 3 This is a schematic diagram illustrating the construction of potential field vector features in the state space of the decision network in Embodiment 1 of the present invention.

[0053] Figure 4 This is a schematic diagram illustrating the principle of dynamic virtual sub-target generation and DWA-guided obstacle avoidance in Embodiment 1 of the present invention.

[0054] Figure 5 This is a trend chart of the training process success rate in Embodiment 1 of the present invention.

[0055] Figure 6 This is a test result diagram in a simulation environment in Embodiment 1 of the present invention.

[0056] Figure 7 This is a schematic diagram of the control module in Embodiment 2 of the present invention. Detailed Implementation

[0057] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0058] Example 1

[0059] This embodiment provides a path planning method for unmanned cleaning vehicles based on dynamic bias. Specifically, it is a path planning method for unmanned cleaning vehicles based on potential field enhancement SAC (Soft Actor-Critic) and dynamic bias DWA (Dynamic Window Approach). This method aims to solve the problems of unmanned cleaning vehicles easily getting trapped in local optima in complex terrains such as U-shaped traps, and the lack of smoothness in end-to-end reinforcement learning control when using traditional DWA algorithms.

[0060] Please see Figures 1 to 4 The path planning method includes steps S1 to S5, namely environmental perception and feature extraction, decision network construction and reasoning, dynamic virtual sub-target generation, and local path planning.

[0061] S1. Obtain the relative coordinates of the current position and the final target point of the unmanned cleaning vehicle, construct a line-of-sight guidance vector from the current position to the final target, and calculate the ideal pre-aiming point position at a preset forward-looking distance along the direction of the vector. At the same time, obtain the obstacle distribution information of the surrounding environment in real time through the airborne lidar.

[0062] Step S1 involves environmental perception and global pre-aiming. In each control cycle (e.g., T=50ms), the system first performs a state update. The global coordinate system is defined as {W}, and the vehicle's coordinate system is defined as {B}. The navigation state is obtained, and the current pose of the unmanned cleaning vehicle in {W} is acquired via odometry and IMU (Inertial Measurement Unit). and the coordinates of the final target point Then calculate the ideal aiming point. Based on the principle of line-of-sight guidance, a system is constructed using... point to The unit guide vector. Calculate the ideal aiming point at a Lookahead distance (e.g., 1.5 meters) from the vehicle's forward viewpoint. :

[0063] ;

[0064] This point This represents the optimal intermediate navigation point under the assumption of no obstacles.

[0065] S2. Construct an environmental feature extraction model based on an artificial potential field. Based on the obstacle distribution information, use the environmental feature extraction model to calculate the target gravity vector and the resultant repulsion vector of the obstacles, and use the gravity vector and the resultant repulsion vector as physical feature vectors.

[0066] This embodiment introduces the Artificial Potential Field (APF) method to perform physical dimensionality reduction on environmental information. The calculation process of the environmental feature extraction model includes S21 to S24.

[0067] S21. Establish the gravitational potential field function and calculate the target gravitational vector pointing from the current position to the final target point. .

[0068] S22. Establish the repulsive potential field function, and calculate the repulsive vector generated by each obstacle point within the sensing range of the airborne lidar.

[0069] Specifically, 2D LiDAR scanning data is acquired, and for each obstacle point Oi within the scanning range, the repulsive force it generates on the unmanned cleaning vehicle is calculated. , where i is the index of the obstacle point. Set the obstacle influence distance threshold. =2.0m. When the obstacle distance < When the repulsive force is calculated, the formula is:

[0070] ;

[0071] S23. Repel the forces generated by all obstacles within the sensing range. By performing vector superposition, the resultant and repulsive force vectors of the obstacles are obtained. .

[0072] S24. Normalize the maximum value of the target gravity vector and the combined repulsion vector of the obstacle so that their values ​​are distributed in the interval [-1,1], and construct a high-dimensional physical feature vector, which serves as the key environmental input for the subsequent neural network.

[0073] S3. Construct a decision network based on SAC, and input the physical feature vector and the motion state of the unmanned cleaning vehicle as the state space into the decision network, and output a two-dimensional position bias for the ideal aiming point.

[0074] The SAC deep reinforcement learning algorithm based on the maximum entropy principle is used to train a policy network (Actor) to dynamically adjust the navigation target.

[0075] First, we design the state space, which represents the input state of the decision network at the current time t. It consists of two parts: one is its own state of motion: the current linear velocity. angular velocity Distance from the pre-aiming point Heading deviation Second, environmental perception characteristics: the gravitational vector components calculated in step S2 ( , ) and the resultant repulsive force vector components ( , Therefore, the state space is defined as:

[0076] ;

[0077] superscript This is the transpose symbol.

[0078] Subsequently, the action space is designed: the network output is defined for the ideal pre-aiming point. Two-dimensional position offset . This represents the offset distance along the longitudinal direction (front and rear) of the vehicle body. This represents the offset distance along the lateral (left and right) direction of the vehicle body. The output layer of the decision network uses the Tanh activation function to restrict actions to a preset range. Inside.

[0079] Finally, the normalized state Input the trained Actor network and output the optimal bias at the current time step. .

[0080] Step S3 of this invention constructs a decision network based on SAC, a method based on deep reinforcement learning. Through trial and error with the environment, it learns the optimal strategy in a continuous state space. Existing technologies include rule-based methods that rely on the designer's pre-exhaustive enumeration of vehicle force conditions. These methods artificially set virtual target points or remove repulsive forces using preset rules, directly using the resultant force direction of the artificial potential field to drive or guide vehicle movement; this is a direct rule-based control approach. In contrast, this invention has stronger generalization ability for unstructured and complex environments that are difficult to define. Furthermore, this invention does not directly use potential field forces to drive the vehicle, but rather uses potential field forces as the state input of the neural network (SAC). This process transforms complex environmental information into low-dimensional physical features, reducing the SAC state space and thus accelerating SAC training convergence. Finally, this invention uses the positional bias of the trained SAC network output relative to the ideal pre-aiming point to generate virtual sub-targets to indirectly guide the DWA algorithm for underlying planning.

[0081] Furthermore, this invention combines the target's gravitational vector and the combined repulsive force vector of obstacles as physical feature vectors, directly using them as input to the neural network's state space. This approach extracts the topological features of the physical environment and inputs them into the neural network, essentially providing the network with prior knowledge. This method accelerates training convergence speed and solves the problem of training difficulties caused by sparse rewards.

[0082] Furthermore, this invention belongs to indirect guidance control. The SAC network outputs a two-dimensional position offset for the ideal aiming point. A virtual target is generated by outputting the offset, and the smoothing characteristics of the DWA algorithm are used to follow the virtual target, achieving a control effect of flexible upper-level decision-making and robust lower-level execution.

[0083] S4. Use the two-dimensional position offset to perform vector correction on the ideal pre-aiming point to generate a dynamic virtual sub-target point.

[0084] This invention utilizes the bias output by the decision network to "bend" the original straight-line pre-aiming path.

[0085] Due to the bias of the decision network output It is based on the vehicle coordinate system {B}, and needs to be transformed to the global coordinate system {W}. Let the current heading angle be... The rotation matrix is ​​R( ).

[0086] Corrected dynamic virtual sub-target points The positional expression is as follows:

[0087] ;

[0088] In the formula, This represents the position of the dynamically generated virtual sub-target point in the global coordinate system at the current moment. Let be the position of the ideal aiming point at the current moment; tanh(·) is the hyperbolic tangent function; Sensitivity coefficients set by the user; , These are the components of the resultant repulsive force vector in the X and Y dimensions, respectively; Let be the heading angle of the unmanned cleaning vehicle at the current time t; This is a rotation matrix used to transform the offset of the unmanned cleaning vehicle from the local coordinate system to the global coordinate system. The two-dimensional position offset is given.

[0089] This invention addresses the problem of unmanned cleaning vehicles struggling to balance path smoothness and obstacle-avoidance capability under complex constraints by constructing the aforementioned formula. It introduces... This nonlinear adjustment term utilizes the saturation characteristics of the hyperbolic tangent function and the repulsive force of the environment. The modulus is used as a coefficient. When the unmanned cleaning vehicle is in an open area, the obstacle repulsion force is almost zero, and the adjustment term approaches 0, which suppresses the bias. At this time, the position of the virtual sub-target is basically consistent with the position of the target point, and the unmanned cleaning vehicle can efficiently track the global straight path through DWA. When the unmanned vehicle approaches an obstacle, the repulsion force modulus increases, the adjustment term approaches 1, and the bias output by the SAC network is activated. At this time, the generated virtual sub-target point causes DWA to abandon the original route and perform obstacle avoidance and detour.

[0090] In the above formula, the bias output by the SAC network is defined based on the vehicle coordinate system, using the rotation matrix formula. By transforming to the global coordinate system, the neural network only needs to learn relative relationships (left / right) and does not need to learn absolute relationships (to the global coordinate system). In complex dynamic environments, global coordinates change frequently, making direct learning of global coordinates extremely difficult. This computational method reduces the high-dimensional global navigation problem to a local relative correction problem. Combined with the potential force vector (also a relative relationship) input in step S2, the SAC network can converge faster, improving the algorithm's training efficiency and environment generalization ability.

[0091] The above calculation method ultimately generates a value that deviates from the actual target point. Traditional DWA algorithms are prone to getting stuck in local optima, tending to move directly towards the target, which can cause vehicles to get stuck in corners in U-shaped wall scenarios. This application adjusts the DWA evaluation function mathematically by calculating a virtual point located at the opening on the side of the obstacle. To chase this high-scoring virtual point, the DWA algorithm naturally calculates an arc-shaped trajectory that bypasses the obstacle. This calculation method utilizes the global perception capability of SAC to compensate for the local shortsightedness of DWA, achieving active escape, while allowing DWA to perform low-level control, retaining the advantages of DWA in trajectory smoothness and safety.

[0092] Finally, this invention employs a locally dynamically generated bias strategy, which can calculate continuous correction vectors through a neural network based on the real-time obstacle distribution, actively pushing the virtual target towards the side opening of the obstacle (e.g., Figure 4 (As shown).

[0093] For example, when the cleaning truck encounters a wall in front (the repulsive vector points backward), the SAC decision network will output a larger lateral force based on experience. This makes the final Move to the open area to the side of the wall.

[0094] S5. Using dynamic virtual sub-target points as local navigation targets, the Dynamic Window Method (DWA) is used for velocity sampling and trajectory evaluation, and the optimal linear velocity and angular velocity are output to control the movement of the unmanned cleaning vehicle.

[0095] In step S5, DWA performs trajectory sampling in the vehicle's velocity space. Multiple velocity pairs are sampled internally, and multiple simulated trajectories are extrapolated over a future period of time (e.g., 2 seconds) based on the kinematic model.

[0096] Specifically, the derivation process of the simulated trajectory is as follows: First, a discrete kinematic model of the unmanned cleaning vehicle is established. It is assumed that within an extremely short time interval... If the unmanned cleaning vehicle moves at a constant speed, then the update formulas for the unmanned cleaning vehicle's position and heading angle are:

[0097] ;

[0098] In the formula, ( , , )and( , , ) are the x-coordinate, y-coordinate, and heading angle of the unmanned cleaning vehicle at the k-th and k+1-th time steps, respectively.

[0099] Each simulated trajectory is scored using an improved evaluation function:

[0100] ;

[0101] In the formula, To achieve the current sampling rate The overall evaluation score is as follows: and These are the linear velocity and angular velocity of the unmanned cleaning vehicle, respectively. This is an azimuth evaluation function used to measure the orientation of the simulated trajectory's end point relative to the dynamic virtual sub-target point. The deviation between the two is derived from the kinematic model of the unmanned cleaning vehicle; This is a distance evaluation function used to measure the distance between the end of the simulated trajectory and the nearest obstacle; For velocity evaluation function; , , These are the corresponding weighting coefficients.

[0102] Select the sampling rate with the highest overall evaluation score. The command is sent to the underlying driver of the unmanned cleaning vehicle for execution.

[0103] Specifically, the formula for calculating the azimuth evaluation function is as follows:

[0104] ;

[0105] In the formula, To achieve the current sampling rate Below, the predicted heading angle of the unmanned cleaning vehicle at the end of its trajectory is obtained through kinematic simulation. Pi is used to normalize angular deviations; The current location of the unmanned cleaning vehicle is pointed to a dynamic virtual sub-target point. The target azimuth angle is calculated using the following formula:

[0106] ;

[0107] In the formula, A function to calculate the arctangent value between two variables; and These are the x and y coordinates of the dynamic virtual sub-target point in the global coordinate system, respectively. and Let x and y be the x and y coordinates of the unmanned cleaning vehicle at the current time t in the global coordinate system.

[0108] The formula for calculating the distance evaluation function is:

[0109] ;

[0110] In the formula, Indicates the current sampling rate The minimum Euclidean distance between all trajectory points and the nearest obstacle detected by the airborne lidar on the predicted trajectory of the pushdown performance; This is the preset safety evaluation threshold or the effective detection range of the airborne lidar; if means if, otherwise means otherwise.

[0111] The formula for calculating the velocity evaluation function is:

[0112] ;

[0113] In the formula, Candidate values ​​for linear velocity in the current sampling group; This refers to the maximum permissible linear speed of the unmanned cleaning vehicle under system settings or physical limitations.

[0114] In this embodiment, the training reward function of the decision network includes the following parts:

[0115] Target proximity reward: Positively correlated with the change in distance of the unmanned cleaning vehicle as it approaches the final target point;

[0116] Collision penalty: A negative reward is given when the distance between the unmanned cleaning vehicle and an obstacle is less than the safe radius;

[0117] Bias penalty : In the formula, and These are the lateral and longitudinal coordinate offsets from the ideal aiming point, respectively, within the coordinate system of the unmanned cleaning vehicle itself. This is the penalty coefficient.

[0118] Therefore, this embodiment also designs a composite reward function, the expression of which is as follows:

[0119] ;

[0120] In the formula, A composite reward for training the decision network; This is a reward for reaching the final goal (e.g., +20); It is a collision penalty (e.g., -20); As a reward for proximity, the unmanned cleaning vehicles are encouraged to approach the finish line. and These are the distances from the destination at time t-1 and time t, respectively. and These are weighting coefficients that can be set independently.

[0121] This embodiment also sets up a simulation environment: built based on ROS2 (Robot Operating System) and the Gazebo physics simulation engine. The simulation scene constructs an obstacle environment (20m x 20m) including a long corridor and alleyways. The kinematic parameters of the unmanned cleaning vehicle are set as follows: maximum linear velocity... =1.5m / s, maximum angular velocity =1.5rad / s; the detection range of the airborne lidar is set to 10m, and the field of view is 360 degrees. The path planning method of the present invention is verified based on this simulation environment.

[0122] like Figure 5 As shown, the success rate trend of the training process in this invention exhibits significant convergence characteristics. The horizontal axis represents the number of training episodes (each episode containing 500 control steps), and the vertical axis represents the navigation success rate. In the first 4000 training episodes, the agent is in an exploration and accumulation phase, resulting in a low success rate. Between 4000 and 8000 episodes, the introduction of the artificial potential field effectively reduces the ineffective search space, allowing the decision network to quickly master obstacle avoidance strategies, and the success rate increases. After 8000 episodes, the success rate stabilizes at a high level above 0.9 (i.e., 90%). This result demonstrates that the method of this invention can effectively converge in large-scale training.

[0123] like Figure 6 As shown, the experimental tests of this invention were conducted in a simulation environment based on the Gazebo physics engine. Figure 6The diagram includes an obstacle cost map (Costmap) that has been inflated based on LiDAR sensing. Line 1 (green) represents the real-time trajectory planned using the method described in this invention. Lines 2 (purple), 3 (red), and 4 / 5 (cyan) represent inflated layers calculated based on the inflated radius. The closer to the obstacle, the higher the cost, with the cost in the red area < the purple area < the cyan area. This is designed to prevent the robot from colliding with obstacles. Line 6 (blue square) represents the unmanned cleaning vehicle. The pink area indicated by the black arrow is a non-passable area, with white dashed lines inside representing radar scan lines. The test results clearly demonstrate that when the unmanned cleaning vehicle approaches an area with dense obstacles, it can plan a smooth, continuous, and safely curved path around the obstacle edges, successfully avoiding collisions and smoothly navigating narrow bends. This verifies the effectiveness of the Dynamic Bias Strategy-Guided DWA algorithm in practical applications.

[0124] Example 2

[0125] This embodiment provides an unmanned cleaning vehicle, including a cleaning vehicle body and a control module mounted on the cleaning vehicle body. The control module includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the steps of the unmanned cleaning vehicle path planning method based on dynamic bias as described in Embodiment 1.

[0126] like Figure 7 As shown, the control module provided in this embodiment includes: at least one processor 101, and a memory 102 connected to at least one processor 101. This embodiment does not limit the specific connection medium between the processor 101 and the memory 102. Figure 7 The example shown is the connection between processor 101 and memory 102 via bus 100. Bus 100 is... Figure 7 The connections between other components are shown in bold lines and are for illustrative purposes only, not as limiting information. Bus 100 can be divided into address bus, data bus, control bus, etc., for ease of representation. Figure 7 The bus is represented by a single thick line, but this does not indicate that there is only one bus or one type of bus. Alternatively, the processor 101 may also be called a controller; there is no restriction on the name.

[0127] In this embodiment, the memory 102 stores instructions that can be executed by at least one processor 101. The at least one processor 101 can execute the aforementioned method by executing the instructions stored in the memory 102.

[0128] The processor 101 is the control center of the device. It can connect to various parts of the control device through various interfaces and lines. By running or executing instructions stored in memory 102 and calling data stored in memory 102, the processor can perform various functions and process data, thereby monitoring the device as a whole.

[0129] In one possible design, processor 101 may include one or more processing units. Processor 101 may integrate an application processor and a modem processor, wherein the application processor mainly handles the operating system, user interface, and applications, and the modem processor mainly handles wireless communication. It is understood that the modem processor may also not be integrated into processor 101. In some embodiments, processor 101 and memory 102 may be implemented on the same chip; in some embodiments, they may also be implemented on separate chips.

[0130] Processor 101 can be a general-purpose processor, such as a central processing unit (CPU), digital signal processor, application-specific integrated circuit, field-programmable gate array or other programmable logic device, discrete gate or transistor logic device, or discrete hardware component, capable of implementing or executing the methods, steps, and logic block diagrams disclosed in the embodiments. The general-purpose processor can be a microprocessor or any conventional processor. The steps of the unmanned cleaning vehicle path planning method based on dynamic bias disclosed in Embodiment 1 can be directly manifested as execution by a hardware processor, or executed by a combination of hardware and software modules in processor 101.

[0131] Memory 102, as a non-volatile computer-readable storage medium, can be used to store non-volatile software programs, non-volatile computer-executable programs, and modules. Memory 102 may include at least one type of storage medium, such as flash memory, hard disk, multimedia card, card-type memory, random access memory (RAM), static random access memory (SRAM), programmable read-only memory (PROM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), magnetic storage, magnetic disk, optical disk, etc. Memory 102 can be any other medium capable of carrying or storing desired program code in the form of instructions or data structures that can be accessed by a computer, but is not limited thereto. In this embodiment, memory 102 can also be a circuit or any other device capable of implementing storage functions for storing program instructions and / or data.

[0132] By designing and programming the processor 101, the code corresponding to the path planning method for the unmanned cleaning vehicle based on dynamic bias described in the aforementioned embodiments can be embedded into the chip, thereby enabling the chip to execute the code during runtime. Figure 1 The steps of the path planning method for the unmanned cleaning vehicle based on dynamic bias are shown. How to design and program the processor 101 is a technique well-known to those skilled in the art and will not be described further here.

[0133] Example 3

[0134] This embodiment provides a computer-readable storage medium storing a computer program thereon. When the program is executed by a processor, it implements the steps of the unmanned cleaning vehicle path planning method based on dynamic bias as described in Embodiment 1.

[0135] The computer-readable storage medium may include flash memory, hard disk, multimedia card, card-type memory (e.g., SD or DX memory), random access memory (RAM), static random access memory (SRAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), programmable read-only memory (PROM), magnetic memory, magnetic disk, optical disk, etc. In some embodiments, the storage medium may be an internal storage unit of a computer device, such as the hard disk or memory of the computer device. In other embodiments, the storage medium may also be an external storage device of the computer device, such as a plug-in hard disk, smart memory card, secure digital card, flash memory card, etc., provided on the computer device. Of course, the storage medium may include both internal storage units and external storage devices of the computer device. In this embodiment, the memory is typically used to store the operating system and various application software installed on the computer device. In addition, the memory can also be used to temporarily store various types of data that have been output or will be output.

[0136] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

Claims

1. A path planning method for unmanned cleaning vehicles based on dynamic bias, characterized in that, Includes the following steps: S1. Obtain the relative coordinates of the current position and the final target point of the unmanned cleaning vehicle, construct a line-of-sight guidance vector from the current position to the final target, and calculate the ideal pre-aiming point position at a preset forward-looking distance along the direction of the vector. At the same time, obtain the obstacle distribution information of the surrounding environment in real time through the airborne lidar. S2. Construct an environmental feature extraction model based on an artificial potential field. Based on the obstacle distribution information, use the environmental feature extraction model to calculate the target gravity vector and the resultant repulsion vector of the obstacles, and use the target gravity vector and the resultant repulsion vector of the obstacles as physical feature vectors. S3. Construct a decision network based on SAC, and input the physical feature vector and the motion state of the unmanned cleaning vehicle as the state space into the decision network, and output the two-dimensional position bias for the ideal pre-aiming point; S4. Using the two-dimensional position offset to perform vector correction on the ideal pre-aiming point to generate a dynamic virtual sub-target point, specifically including: transforming the two-dimensional position offset from the local coordinate system of the unmanned cleaning vehicle to the global coordinate system to obtain a global offset vector; The magnitude of the resultant and repulsive force vector of the obstacle is calculated, and the magnitude is mapped using the hyperbolic tangent function to obtain a nonlinear adjustment coefficient. The global bias vector is weighted using the nonlinear adjustment coefficient, and the weighted vector is superimposed on the position of the ideal pre-aiming point to obtain a dynamic virtual sub-target point. S5. Using dynamic virtual sub-target points as local navigation targets, the dynamic window method is used for velocity sampling and trajectory evaluation, and the optimal linear velocity and angular velocity are output to control the movement of the unmanned cleaning vehicle.

2. The path planning method for unmanned cleaning vehicles based on dynamic bias according to claim 1, characterized in that, In step S2, the calculation process of the environmental feature extraction model includes: S21. Establish the gravitational potential field function and calculate the target gravitational vector pointing from the current position to the final target point; S22. Establish the repulsive potential field function, and calculate the repulsive force vector generated by each obstacle point within the sensing range of the airborne lidar; S23. Superimpose the repulsive force vectors generated by all obstacles within the sensing range to obtain the resultant repulsive force vector of the obstacles; S24. Normalize the target gravity vector and the combined repulsion vector of the obstacle to construct a physical feature vector.

3. The path planning method for unmanned cleaning vehicles based on dynamic bias according to claim 1, characterized in that, In step S3, the state space is defined as follows: In the formula, Represents the state space at the current time t; The current linear speed of the unmanned cleaning vehicle; The current angular velocity of the unmanned cleaning vehicle; The relative distance between the current position of the unmanned cleaning vehicle and the ideal aiming point; The relative angle between the current position of the unmanned cleaning vehicle and the ideal aiming point; , These are the components of the gravitational vector in the X and Y dimensions, respectively; , These are the components of the resultant repulsive force vector in the X and Y dimensions, respectively; superscript This is the transpose symbol.

4. The path planning method for unmanned cleaning vehicles based on dynamic bias according to claim 1, characterized in that, In step S3, the action space of the decision network is defined as the following continuous action vectors: In the formula, The action space at the current time t is the two-dimensional position offset relative to the ideal aiming point; and These represent the lateral and longitudinal coordinate offsets from the ideal pre-aiming point, respectively, within the coordinate system of the unmanned cleaning vehicle; the output layer of the decision network uses the Tanh activation function to restrict the actions to a preset range. Inner; superscript This is the transpose symbol.

5. The path planning method for unmanned cleaning vehicles based on dynamic bias according to claim 1, characterized in that, In step S4, the position expression of the dynamic virtual sub-target point is as follows: In the formula, This represents the position of the dynamically generated virtual sub-target point in the global coordinate system at the current moment. Let be the position of the ideal aiming point at the current moment; tanh(·) is the hyperbolic tangent function; The preset sensitivity coefficient; , These are the components of the resultant repulsive force vector in the X and Y dimensions, respectively; Let be the heading angle of the unmanned cleaning vehicle at the current time t; This is a rotation matrix used to transform the offset of the unmanned cleaning vehicle from the local coordinate system to the global coordinate system. The two-dimensional position offset is given.

6. The path planning method for unmanned cleaning vehicles based on dynamic bias according to claim 1, characterized in that, In step S5, the evaluation function of the dynamic window method is: In the formula, To achieve the current sampling rate The overall evaluation score is as follows: and These are the linear velocity and angular velocity of the unmanned cleaning vehicle, respectively. This is an azimuth evaluation function used to measure the orientation of the simulated trajectory's end point relative to the dynamic virtual sub-target point. The deviation between the two is derived from the kinematic model of the unmanned cleaning vehicle; This is a distance evaluation function used to measure the distance between the end of the simulated trajectory and the nearest obstacle; For velocity evaluation function; , , These are the corresponding weighting coefficients; Among them, the sampling speed with the highest overall evaluation score was selected. The command is sent to the underlying driver of the unmanned cleaning vehicle for execution.

7. The path planning method for unmanned cleaning vehicles based on dynamic bias according to claim 6, characterized in that, The formula for calculating the azimuth evaluation function is: In the formula, To achieve the current sampling rate Below, the predicted heading angle of the unmanned cleaning vehicle at the end of its trajectory is obtained through kinematic simulation. Pi is used to normalize angular deviations; The current location of the unmanned cleaning vehicle is pointed to a dynamic virtual sub-target point. The target azimuth angle is calculated using the following formula: In the formula, A function to calculate the arctangent value between two variables; and These are the x and y coordinates of the dynamic virtual sub-target point in the global coordinate system, respectively. and These are the x and y coordinates of the unmanned cleaning vehicle in the global coordinate system at the current time t; The formula for calculating the distance evaluation function is: In the formula, Indicates the current sampling rate The minimum Euclidean distance between all points on the simulated trajectory and the nearest obstacle detected by the airborne lidar; This is to preset the safety evaluation threshold or the effective detection range of the airborne lidar; The formula for calculating the velocity evaluation function is: In the formula, Candidate values ​​for linear velocity in the current sampling group; This refers to the maximum permissible linear speed of the unmanned cleaning vehicle under system settings or physical limitations.

8. The path planning method for unmanned cleaning vehicles based on dynamic bias according to claim 1, characterized in that, In step S3, the training reward function of the decision network includes the following parts: Target proximity reward: Positively correlated with the change in distance of the unmanned cleaning vehicle as it approaches the final target point; Collision penalty: A negative reward is given when the distance between the unmanned cleaning vehicle and an obstacle is less than the safe radius; Bias penalty : In the formula, and These are the lateral and longitudinal coordinate offsets from the ideal aiming point, respectively, within the coordinate system of the unmanned cleaning vehicle itself. This is the penalty coefficient.

9. An unmanned cleaning vehicle, comprising a cleaning vehicle body and a control module mounted on the cleaning vehicle body, the control module comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the unmanned cleaning vehicle path planning method based on dynamic bias as described in any one of claims 1 to 8.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the steps of the unmanned cleaning vehicle path planning method based on dynamic bias as described in any one of claims 1 to 8.

Images