Path planning method based on target characteristics

Abstract

The invention relates to the technical field of path planning, in particular to a path planning method based on target characteristics. The motion instability caused by steering sudden change is effectively inhibited by adopting a tightening window and strengthening path cost for a large obstacle; and the local obstacle avoidance flexibility is improved by relaxation constraint of small obstacles. The newly added braking safety verification ensures that no sudden stop risk exists under sudden obstacles, experimental data shows that the scheme synchronously optimizes the path length, task time, path smoothness and safety in a complex scene, meets the core requirement of efficient and stable operation of the commercial scrubber, adopts a tightening strategy for large obstacles, and adopts a tightening strategy for small obstacles. If yes, the constraint is relaxed, the path penalty weight is reduced, more aggressive small-range maneuvering is allowed, and the scene adaptability is remarkably improved through the mechanism; the technical problems that when the scrubber adopts a traditional DWA as a local algorithm to conduct path planning, the track is not smooth, the path is roundabout, and braking safety is insufficient are solved.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to a path planning method based on target characteristics. Background Art

[0002] With the construction of smart cities, commercial floor-cleaning robots have gradually become an important tool in the modern cleaning field. Floor-cleaning robot navigation usually consists of positioning, mapping, perception, and path planning. Planning can be divided into local path planning and global path planning. Global path planning generates a collision-free path from the starting point to the end point based on the established map. Local path planning updates the path in real time based on sensor information and handles obstacles added to the map. The dynamic window algorithm (DWA) strictly adheres to the speed, acceleration, and steering capabilities that a real robot can achieve in planar motion. Therefore, it is widely used in local path planning.

[0003] However, in the operating scenarios of floor scrubbing robots, traditional DWA algorithms still have the following shortcomings: First, the fixed parameterization strategy of traditional algorithms ignores the differences in the physical properties of obstacles. As a result, when the robot faces large obstacles (such as shelves and walls), the robot may cause motion path jitter due to sudden changes in steering angles, affecting the stability of the robot's motion; second, in dense mixed obstacle scenario systems, the fixed dynamic window size may cause redundant path detours or increased energy consumption, affecting cleaning coverage efficiency; third, the failure to embed braking safety verification in the trajectory generation link may cause emergency stops or path oscillations due to improper speed planning when sudden obstacles intervene, threatening the safety of the floor scrubber equipment and pedestrians. Summary of the Invention

[0004] In response to the shortcomings of the existing technology, the present invention provides a path planning method based on target characteristics, which solves the technical problems of uneven trajectory, circuitous path and insufficient braking safety when the floor scrubber adopts traditional DWA as a local algorithm for path planning.

[0005] To solve the above technical problems, the present invention provides the following technical solution: a path planning method based on target characteristics, the path planning method specifically comprising the following steps: Map construction: Use LiDAR to obtain the 3D point cloud of the work scene in real time, intercept the 3D point cloud within a predetermined range, and use radius filtering to construct a 2D grid map; Positioning and map updating: Obtain the coordinates of the robot's starting point on the 2D grid map, and generate and update the cost map in real time based on the 3D point cloud and 2D grid map acquired by the radar in real time; Obstacle classification: Based on the logical threshold and the grid cost value of the 2D grid map, it is compared with the corresponding expansion area cost threshold of the cost map to identify, classify and pre-store obstacles in the 2D grid map to obtain obstacle characteristic data; Path planning: Plans the robot's global path from the starting point to the target point on a 2D grid map, and plans the robot's temporary trajectory in real time based on obstacle characteristic data and the DWA algorithm; Target point judgment: judge whether the robot has reached the target point; If so, then end; If not, return to positioning and map update.

[0006] Preferably, the map construction specifically includes the following steps: S21, 3D point cloud of the operation scene based on laser radar collection; S22, intercepting 3D point cloud data within a range of height values ​​within a range of interest in the z-axis direction; S23, using a radius filtering method to remove dynamic outliers in the intercepted 3D point cloud data to obtain a 2D grid map.

[0007] Preferably, in obstacle classification, the method for identifying and classifying obstacles in a 2D grid map is as follows: if the grid cost value exceeds the expansion area cost threshold of the cost map, the grid is determined to be an obstacle, otherwise it is a non-obstacle; when the number of consecutive grids occupied by the obstacle exceeds the logical threshold of the obstacle size, it is determined to be a large obstacle, otherwise it is a small obstacle.

[0008] Preferably, the path planning specifically includes the following steps: S41, planning the global path of the robot from the starting point to the target point; S42. Use the four-wheel two-drive differential motion model to construct the robot's motion model in the global coordinate system. The expression of the motion model is: In the above formula, and are the instantaneous velocity components of the robot's center of mass in the x-axis and y-axis directions of the global coordinate system, is the robot orientation, that is, the angle between the robot's center of mass and the vehicle's head and the x-axis in the global coordinate system. is the rate of change of the heading angle with time, that is, the angular velocity of the robot around the axis perpendicular to the plane of the global coordinate system; S43, sampling the speed and angular velocity according to the current speed and angular velocity of the robot to obtain a feasible speed space; S44. Scaling the maximum linear velocity and maximum angular velocity of the robot's driving wheels according to the obstacle characteristic data, and regenerating a feasible new velocity space according to the scaled maximum linear velocity and maximum angular velocity; S45, selecting a linear velocity and an angular velocity in a feasible new velocity space, combining them and using them as the motion state of the robot, and predicting the predicted trajectory of the robot in the next period of time; S46. Calculate the braking distance between the robot and the nearest obstacle at each trajectory point of each predicted trajectory in sequence. The calculation formula is: In the above formula, and They represent the braking distances of the robot at time t and time t-1 respectively, and They represent the linear velocity of the robot at time t and time t-1 respectively, a represents the acceleration of the robot, To control the cycle, the loop is terminated when v≤0; S47, sequentially determining whether there is a trajectory point on each predicted trajectory where the braking distance between the robot and the nearest obstacle is greater than or equal to the actual distance; If so, the predicted trajectory is eliminated; If not, mark the predicted trajectory as a candidate trajectory; S48. Evaluate the candidate trajectories through the evaluation model, and select the candidate trajectory with the highest evaluation score as the temporary trajectory for the robot to move.

[0009] Preferably, in step S44, the specific method of scaling the maximum linear velocity and maximum angular velocity of the robot driving wheel according to the obstacle characteristic data is: obtaining the obstacle characteristic data, and updating the maximum linear velocity and maximum angular velocity of the robot driving wheel, and the updating formula is: in, and Represent the maximum linear velocity and maximum angular velocity of the updated robot respectively. For large obstacles, the excitation factor <1, for small obstacles, the excitation factor >1.

[0010] Preferably, in step S48, the expression of the evaluation model is: in, represents the evaluation score of the evaluation model, and represents the linear velocity and angular velocity that the robot can move. is the heading angle evaluation item, which quantifies the deviation between the candidate trajectory direction and the target point orientation. Indicates the minimum Euclidean distance between the trajectory point and the nearest obstacle. The larger the distance, the higher the safety margin. It is an evaluation item of motion efficiency. Reflects the linear velocity amplitude, used to improve motion efficiency, parameters 、 and are the first, second and third weight coefficients respectively, It is a nonlinear adjustment factor used to balance the contribution of each sub-item. Size sensitivity factor , for large obstacles, >1, for small obstacles <1.

[0011] Preferably, in step S43, the expression of the feasible speed space is: in, in, represents the feasible velocity space, represents the speed constraint based on the physical limits of linear and angular velocity of the drive system's mechanical performance. represents the speed constraint limited by the maximum acceleration and deceleration of the drive system, Represents the safe braking speed constraint based on the minimum distance between the robot's moving trajectory point and the obstacle, and represents the linear velocity and angular velocity that the robot can move. and are the minimum and maximum linear speeds of the robot’s driving wheels, and are the minimum and maximum angular velocities of the driving wheel, and are the linear velocity and angular velocity of the robot in the current state, and are the maximum linear acceleration and maximum angular acceleration of the robot, To control the cycle, is the shortest distance between the robot and the obstacle.

[0012] By means of the above technical solution, the present invention provides a path planning method based on target characteristics, which has at least the following beneficial effects: 1. This invention effectively suppresses motion instability caused by sudden steering changes by employing a tighter window and enhanced path cost for large obstacles. It also relaxes constraints for small obstacles to enhance local obstacle avoidance flexibility. New braking safety verification ensures no risk of sudden stops under unexpected obstacles. Experimental data demonstrates that this solution simultaneously optimizes path length, mission time, path smoothness, and safety in complex scenarios, meeting the core requirements for efficient and stable operation of commercial floor scrubbers.

[0013] 2. The present invention achieves dynamic fine-tuning of motion parameters through quantitative grading of the physical properties of obstacles. For large obstacles, a tightening strategy is adopted: the maximum linear velocity and angular velocity are scaled by the excitation factor α<1, limiting the robot's motion range in single-step decision-making and avoiding path oscillation due to inertia. At the same time, a size-sensitive factor is set in the evaluation function to strengthen the penalty weights of heading deviation and obstacle distance, forcing the robot to plan a smooth detour path in advance. For small obstacles, the constraints are relaxed and the path penalty weights are reduced, allowing more aggressive small-scale maneuvers. This mechanism significantly improves scene adaptability. In dense mixed obstacle environments, path redundancy is reduced, obstacle avoidance efficiency is improved, and the standard deviation of angular velocity fluctuations is reduced by 45%, fundamentally solving the path tortuosity and jitter problems caused by fixed parameters in traditional DWA.

[0014] 3. The present invention innovatively embeds a braking safety verification module based on numerical integration in the predicted trajectory generation process. By calculating the dynamic braking distance of each point on the predicted trajectory in real time and comparing it with the actual obstacle distance, the predicted trajectory with collision risk is pre-screened. This module fully considers the high inertia characteristics of the floor scrubber and ensures that the robot can decelerate and stop within a safe distance when a sudden obstacle intervenes. Experiments show that the improved algorithm stably maintains the minimum obstacle distance at a safety threshold of 0.17±0.01m, and the number of collisions is reduced to zero. Traditional DWA collisions occur due to the lack of this mechanism. This design avoids sudden stops or path oscillations caused by improper speed planning at the front end of the control chain, significantly improving the safety of equipment and pedestrians. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] The drawings described herein are used to provide a further understanding of the present application and constitute a part of the present application. The illustrative embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings: Figure 1 Flowchart of the path planning method based on target characteristics of the present invention; Figure 2 Electronic maps of three types of obstacles for simulation experiments; Figure 3 The actual experimental scene and the corresponding 3D points and 2D maps; Figure 4 This is a schematic diagram of the motion model of the four-wheel two-drive differential vehicle of the present invention; Figure 5 A schematic diagram of the trajectory adopted by the trolley of the present invention; Figure 6 This is a schematic diagram of obstacle determination according to the present invention; Figure 7 This is a schematic diagram of distance judgment according to the present invention; Figure 8 Plan path diagrams for the two algorithms under scenario 1; Figure 9 This is the change diagram of the minimum obstacle distance of the two algorithms under scenario 1; Figure 10 The heading change rate diagram of the two algorithms under scenario 1; Figure 11 This is the path curvature change diagram of the two algorithms under scenario 1; Figure 12 Graph showing the curvature change rate of the paths of the two algorithms under scenario 1 Figure 13 This is the simulation related diagram of the traditional algorithm under scenario 2; Figure 14 This is the simulation correlation diagram of the improved algorithm under scenario 2; Figure 15 This is the simulation correlation diagram of the traditional algorithm under scenario 3; Figure 16 This is the simulation correlation diagram of the improved algorithm under scenario 3; Figure 17 This is the Rviz diagram of the traditional DWA algorithm; Figure 18 This is the Rviz diagram of the improved DWA algorithm of the present invention. DETAILED DESCRIPTION

[0016] To make the above-mentioned objectives, features, and advantages of the present invention more clearly understood, the present invention is further described below in detail with reference to the accompanying drawings and specific embodiments. This will enable a full understanding of how this application uses technical means to solve technical problems and achieve technical effects, and to implement the invention accordingly.

[0017] To address the problems of uneven trajectories, circuitous paths, and insufficient braking safety in the traditional dynamic window algorithm (DWA) in commercial floor scrubber operation scenarios, this paper provides a path planning method based on target characteristics by performing obstacle avoidance classification based on target characteristics and combining it with a dynamic window. The paper first establishes a comparison standard between obstacle size and robot body size to form a quantitative classification criterion: for large obstacles, a dynamic window parameter tightening and path cost weighting enhancement strategy are implemented to suppress sharp turn jitter; for small obstacles, the window constraints are relaxed and the path penalty is reduced to improve obstacle avoidance flexibility. A braking safety margin verification module based on numerical integration is introduced before the trajectory evaluation layer to avoid control instability caused by sudden speed changes by pre-screening candidate trajectories with insufficient braking distance. Then, by constructing a "obstacle size-dynamic window-path cost" coupled optimization model, adaptive mapping of environmental characteristics and motion strategies is achieved. Finally, to verify the effectiveness of the algorithm, a multi-obstacle virtual scene is set up, and simulation is carried out with path length, angular velocity standard deviation, curvature standard deviation, the shortest distance between the robot and the obstacle, the number of collisions and task time as evaluation indicators. A real-life test is carried out on a differential chassis prototype based on the ROS platform. Simulation and experimental results show that the improved algorithm is superior to the traditional DWA algorithm in terms of time, path length, path smoothness and other aspects.

[0018] Map construction: In the simulation experiment, the obstacle distribution in the working scene can be generally divided into three scenarios: large obstacles and small obstacles are randomly distributed, large obstacles are concentrated in the middle and small obstacles are distributed on the periphery, and small obstacles are concentrated in the middle of the scene and large obstacles are scattered around. Therefore, the present invention will create three scenarios for analysis respectively. Figure 2 In the figure, black represents obstacles, red dots represent target points, blue lines represent paths, and green fan-shaped windows represent velocity space; In actual operation, since the operating scene of commercial floor scrubbers usually has both large and small obstacles, it is necessary to create Figure 3 The obstacle scene shown in the figure shows that the stool is regarded as a small obstacle, while the table, wall and chairs side by side are identified as large obstacles, thereby meeting the requirements for large and small obstacles in the scene.

[0019] When the floor scrubbing robot moves, the 3D laser point cloud information of the working scene can be collected by using the Leishen C16 3D laser radar, and the 3D spatial point data can be obtained through the 3D SLAM technology of the laser point cloud and the 3D laser sensor. Then, the pose is estimated by scanning and matching between adjacent point cloud frames. The closed-loop detection and graph optimization technology are further used to establish a complete 3D point cloud. Then, the 3D point cloud data within the height value range of the range of interest in the z-axis direction is intercepted, and the dynamic outliers in the 3D point cloud data are further eliminated using the radius filtering method to obtain a 2D grid map, such as Figure 4 As shown in the figure, black represents obstacles. It can be seen from the figure that the obstacles in the processed 2D grid map are clear and can be used for path planning. When used for the first time, a global 2D grid map will be established through the robot radar. In the subsequent working process of the robot, the 3D point cloud is obtained in real time through the radar scanning configured on the robot to modify the 2D grid map to obtain the cost map. The initial 2D grid map and the real-time updated cost map will be retained in the robot's internal system, thereby realizing the acquisition of new temporary obstacles and updating them on the cost map.

[0020] Positioning and map update: Obtain the current position coordinates of the robot on the 2D grid map, which is the coordinates of the starting point. Common algorithms include SLAM algorithms. As the robot moves, the 3D point cloud and 2D grid map obtained in real time by the radar are used to generate and update the cost map in real time.

[0021] Obstacle classification: Based on the expansion area cost threshold of the cost map corresponding to the 2D grid map, the grid cost value of each grid in the 2D grid map is compared. If the grid cost value exceeds the expansion area cost threshold of the cost map, the grid is determined to be an obstacle, otherwise it is a non-obstacle. When the number of consecutive grids occupied by the obstacle exceeds the logical threshold of the obstacle size, it is determined to be a large obstacle, otherwise it is a small obstacle. The classified obstacles are stored; Path planning steps: First, the RRT* algorithm is used for global path planning. Its core idea is to not only quickly build a tree structure covering the space in the process of exploring the space, but also gradually optimize the path so that the final path is close to the optimal cost. The RRT* algorithm randomly samples in the state space and gradually expands from existing nodes to random sampling points to build a rapidly expanding tree. Each time sampling occurs, the algorithm finds the node closest to the sampling point in the current tree, and then extends a short distance from the node toward the sampling point to generate a new node. If there is no collision on the path, the new node is added to the tree and connected to the nearest existing node to obtain the global path.

[0022] Afterwards, in order to use the DWA algorithm to convert position control into speed control, it is necessary to first analyze the robot's motion model. The kinematic model of the robot of the present invention is as follows: Figure 4 As shown in the figure, a four-wheel two-wheel drive differential motion model is used. Assuming that the vehicle displacement is very small during the sampling time and the moving trajectory is considered to be a straight line, the mathematical expression of the motion model is: In the above formula, and are the instantaneous velocity components of the robot's center of mass in the x-axis and y-axis directions of the global coordinate system, is the robot orientation, that is, the angle between the robot's center of mass to the vehicle's front and the x-axis in the global coordinate system. is the rate of change of the heading angle with time, that is, the angular velocity of the robot around the axis perpendicular to the plane of the global coordinate system.

[0023] Then, the DWA algorithm can be used to sample the speed and angular velocity according to the current speed and angular velocity of the car, and calculate the feasible speed sampling space. The constraints that need to be met during the sampling process are: speed constraints based on the physical limits of the linear velocity and angular velocity of the mechanical performance of the drive system , limited by the speed constraints of the drive system's maximum acceleration and deceleration , speed constraint for safe braking based on the minimum distance between trajectory points and obstacles , the expressions of the three are: in, and represents the linear velocity and angular velocity that the robot can move. and are the minimum and maximum linear speeds of the robot’s driving wheels, and are the minimum and maximum angular velocities of the driving wheel, and are the linear velocity and angular velocity of the robot in the current state, and are the maximum linear acceleration and maximum angular acceleration of the robot, To control the cycle, is the shortest distance between the robot and the obstacle.

[0024] Then the feasible speed space of the car can be expressed as: .

[0025] Finally, the speed and angular velocity in the feasible speed space are combined as the motion state of the robot, and the motion trajectory in a certain simulation time is predicted. The obtained sampling trajectory is as follows Figure 5 As shown, further, after obtaining the sampled trajectory of the car, it is necessary to evaluate the predicted path through the evaluation function, and select the state of the car corresponding to the path with the highest evaluation score as the next running state of the robot. The evaluation function expression is shown in the following formula: in, represents the evaluation score of the evaluation model, and represents the linear velocity and angular velocity that the robot can move. is the heading angle evaluation item, which quantifies the deviation between the candidate trajectory direction and the target point orientation. Indicates the minimum Euclidean distance between the trajectory point and the nearest obstacle. The larger the distance, the higher the safety margin. It is a motion efficiency evaluation item. Reflects the linear velocity amplitude, used to improve motion efficiency, parameters 、 and are the first, second and third weight coefficients respectively, It is a nonlinear adjustment factor used to balance the contribution of each sub-item.

[0026] Due to the complex dynamics of the floor scrubber operation scene, relying solely on the RRT* algorithm would result in insufficient local obstacle avoidance. Therefore, it is used as a global plan and the DWA algorithm is used to avoid temporary dynamic obstacles. As the robot moves, it determines obstacles within a certain range of the surrounding area based on the obtained 2D grid map.

[0027] The traditional DWA algorithm mentioned above does not consider the obstacle type when performing obstacle safety assessment during path planning. It creates an obstacle table for all obstacles on the map and uses it to make braking safety judgments for the assumed robot state.

[0028] However, in actual movement, obstacles of different sizes have different effects on the vehicle, and since the scrubber itself has large inertia, insufficient braking safety distance may endanger the safety of the scrubber itself and pedestrians.

[0029] Therefore, the present invention improves the traditional DWA algorithm and first classifies the obstacles into Figure 6, a conservative strategy is implemented for large obstacles, that is, narrowing the dynamic window parameter range and increasing the path cost weight, while a relatively flexible strategy is adopted for small obstacles to optimize the smoothness of the path and improve the passing efficiency of the scene. When the obstacle is a small obstacle, the dynamic window is enlarged, that is, the maximum linear velocity and maximum angular velocity of the robot are enlarged to ensure that the robot reacts quickly and bypasses small obstacles, thereby improving its mobility. The update formula for the maximum linear velocity and maximum angular velocity of the robot's driving wheels is: in, and Represent the maximum linear velocity and maximum angular velocity of the updated robot respectively. For large obstacles, the excitation factor <1, which limits the robot's ability to move. For small obstacles, the excitation factor >1.

[0030] Afterwards, the updated maximum linear velocity and maximum angular velocity of the robot are substituted into the previous formula to regenerate a new velocity space. The predicted trajectory is generated based on the new velocity space using the DWA algorithm, and the trajectory safety is predicted through numerical integration: Assuming that the linear velocity of the car is v and the acceleration is a, where v and a are both discrete sampling values, the iterative formula for the braking distance of the car to the nearest obstacle is: In the above formula, and They represent the braking distances of the robot at time t and time t-1 respectively, and They represent the linear velocity of the robot at time t and time t-1 respectively, a represents the acceleration of the robot, To control the cycle, the loop is terminated when v≤0. In a dynamic environment, the movement and changes of obstacles are unpredictable. Calculating the braking distance in advance can make safer and real-time adjustments to the current state, avoiding sudden changes in speed or path of the robot, thereby reducing control instability.

[0031] Then, it is determined in turn whether there is a trajectory point on each predicted trajectory where the braking distance between the robot and the nearest obstacle is greater than or equal to the actual distance. If so, the predicted trajectory is eliminated; if not, the predicted trajectory is marked as a candidate trajectory.

[0032] Finally, the candidate trajectory is passed to the evaluation module by introducing the size-sensitive factor Improve the evaluation function in the traditional DWA algorithm. For large obstacles, >1, for small obstacles <1, then the expression of the improved evaluation function is: Finally, it is determined whether the robot has reached the target point; if so, the navigation ends; if not, it returns to positioning and map update, and the cycle continues.

[0033] Verification Example 1: DWA Algorithm Simulation Comparison In a scene with randomly distributed obstacles, set the target point and the robot starts planning from the starting point. Figure 8 It can be seen from the figure that the traditional DWA algorithm has obvious turning points when planning the path, the safe distance from obstacles is relatively redundant and the obstacle avoidance fails; while the path generated by the improved algorithm has significantly improved smoothness and can achieve safe detours near obstacles. It meets the requirements of floor scrubber operation. Figure 9 It can be seen from the figure that the traditional algorithm has a collision, while the improved algorithm has improved the path compactness while maintaining a collision safety threshold of 0.17±0.01. The experimental results verify that the improved algorithm optimizes the balance between safety and path efficiency.

[0034] In order to quantitatively evaluate the smoothness of the path, the present invention constructs a dual judgment model based on the curvature standard deviation and the angular velocity standard deviation. The angular velocity standard deviation reflects the heading change rate (such as Figure 9 The curvature is the rotation rate of the path tangent direction to the arc length, which reflects the local bending degree of the path (R is the curvature radius); the standard deviation of the curvature reflects the volatility of the curvature.

[0035] Figure 10 is the heading change rate of the path, Figure 11 and Figure 12 They are the path curvature change graph and the path curvature change rate graph respectively. Figure 10-12 It can be seen that both the traditional algorithm and the improved algorithm experience angular velocity peaks in the initial stage (0-5 seconds) and the path curvature is large. This is essentially the active steering adjustment requirement for path planning. The high angular velocity in this stage is a controlled active adjustment behavior, which conforms to the optimal control logic under kinematic constraints, rather than a stability issue.

[0036] The improved algorithm constrains the dynamic window parameters and adjusts the path cost weight according to the obstacle size. Figure 11 The traditional algorithm graph indexes 250 to 300), which limits the robot's selectable angular velocity range in single-step decision-making. This constrains the steering range of candidate paths. Dynamic adjustment of path costs enables the algorithm to proactively avoid high-curvature paths, simultaneously reducing both the mean and extreme curvature values. The experimental metrics are shown in Table 1: Table 1 Comparison of various indicators of DWA algorithm before and after improvement As can be seen from Table 1, in this scenario, the path time, path length, angular velocity standard deviation, and curvature standard deviation of the improved algorithm are all reduced. From the performance indicators, it can be seen that the performance of the improved algorithm is better than that of the original algorithm.

[0037] Comparative experiments were conducted in scenarios 2 and 3. The simulation diagram of the traditional algorithm in scenario 2 is as follows: Figure 13 , the simulation diagram of the improved algorithm is as follows Figure 14 , the performance indicators are shown in Table 2.

[0038] Table 2 Comparison of various indicators of the DWA algorithm before and after improvement (Scenario 2) The simulation diagram of the traditional algorithm in scenario 3 is as follows Figure 15 , the simulation diagram of the improved algorithm is as follows Figure 16 , the performance indicators are shown in Table 3.

[0039] Table 3 Comparison of various indicators of the DWA algorithm before and after improvement (Scenario 3) As can be seen from Table 3, when both algorithms reach the target point, the minimum distance of the traditional algorithm is lower than the safety distance threshold, and a collision is almost caused. However, the improved algorithm maintains the safety distance threshold while maintaining a smaller distance, and the path length, path time, angular velocity standard deviation, and curvature standard deviation are all reduced.

[0040] Verification Example 2: Comparison of Actual Scenario Experiments Since commercial floor scrubbers usually use four-wheel two-wheel drive differential vehicles as mobile platforms, in order to verify whether the improved algorithm can be used for floor scrubber obstacle avoidance, the present invention will use a four-wheel two-wheel drive vehicle prototype equipped with a Leishen C16 3D laser radar to Figure 2 An autonomous obstacle avoidance experiment was conducted in the scene shown.

[0041] Experimental RVIZ diagram as shown Figure 17 and Figure 18 As shown, Figure 17 This is the RVIZ diagram of the traditional DWA algorithm. Figure 18 To improve the DWA algorithm, we used the RVIZ plot. RVIZ is a visualization software that clearly shows the robot's quantized path from the starting point to the end point. The blue line in the figure represents the robot's actual integral path, the green line represents the feasible path during DWA sampling, and the green border represents the robot's outline. Black represents obstacles, and gray represents the expansion area of ​​the cost map. Path costs increase when approaching gray areas, thereby allowing the robot to plan its path away from obstacles. Table 4 shows the performance metrics of the two algorithms in the obstacle avoidance experiment.

[0042] Table 4 Performance indicators of DWA algorithm obstacle avoidance experiment before and after improvement from Figure 18 As can be seen from Table 4, due to the dynamic adjustment of the cost, the path length, the minimum distance to the obstacle under the threshold conditions, the standard deviation of angular velocity, and the standard deviation of curvature of the improved algorithm are all reduced, and the path time is greatly shortened, which improves the path planning efficiency of the floor scrubber.

[0043] Those skilled in the art will appreciate that all or part of the steps in the above-mentioned embodiment methods can be accomplished by instructing the relevant hardware through a program. Therefore, the present application may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Furthermore, the present application may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0044] Each embodiment in this specification is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to in detail. For the above embodiments, since they are basically similar to the method embodiments, the description is relatively simple. For relevant parts, please refer to the partial description of the method embodiments.

[0045] The above embodiments provide a detailed introduction to the present invention. Specific examples are used herein to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core ideas. At the same time, for those skilled in the art, according to the ideas of the present invention, there may be changes in the specific implementation methods and application scopes. In summary, the contents of this specification should not be understood as limiting the present invention.

Claims

1. A path planning method based on target characteristics, characterized in that: The path planning method specifically includes the following steps: Map construction: Use LiDAR to obtain the 3D point cloud of the work scene in real time, intercept the 3D point cloud within a predetermined range, and use radius filtering to construct a 2D grid map; Positioning and map updating: Obtain the coordinates of the robot's starting point on the 2D grid map, and generate and update the cost map in real time based on the 3D point cloud and 2D grid map acquired by the radar in real time; Obstacle classification: Based on the logical threshold and the grid cost value of the 2D grid map, it is compared with the corresponding expansion area cost threshold of the cost map to identify, classify and pre-store obstacles in the 2D grid map to obtain obstacle characteristic data; Path planning: Plans the robot's global path from the starting point to the target point on a 2D grid map, and plans the robot's temporary trajectory in real time based on obstacle characteristic data and the DWA algorithm; Target point judgment: judge whether the robot has reached the target point; If so, then end; If not, return to positioning and map update.

2. The path planning method according to claim 1, characterized in that: Map construction includes the following steps: S21, 3D point cloud of the operation scene based on laser radar collection; S22, intercepting 3D point cloud data within a range of height values ​​within a range of interest in the z-axis direction; S23, using a radius filtering method to remove dynamic outliers in the intercepted 3D point cloud data to obtain a 2D grid map.

3. The path planning method according to claim 1, wherein: In obstacle classification, the method for identifying and classifying obstacles in a 2D grid map is as follows: if the grid cost value exceeds the expansion area cost threshold of the cost map, the grid is determined to be an obstacle, otherwise it is a non-obstacle. When the number of consecutive grids occupied by the obstacle exceeds the logical threshold of the obstacle size, it is determined to be a large obstacle, otherwise it is a small obstacle.

4. The path planning method according to claim 1, wherein: Path planning includes the following steps: S41, planning the global path of the robot from the starting point to the target point; S42. Use the four-wheel two-drive differential motion model to construct the robot's motion model in the global coordinate system. The expression of the motion model is: In the above formula, and are the instantaneous velocity components of the robot's center of mass in the x-axis and y-axis directions of the global coordinate system, is the robot orientation, that is, the angle between the robot's center of mass and the vehicle's head and the x-axis in the global coordinate system. is the rate of change of the heading angle with time, that is, the angular velocity of the robot around the axis perpendicular to the plane of the global coordinate system; S43, sampling the speed and angular velocity according to the current speed and angular velocity of the robot to obtain a feasible speed space; S44. Scaling the maximum linear velocity and maximum angular velocity of the robot's driving wheels according to the obstacle characteristic data, and regenerating a feasible new velocity space according to the scaled maximum linear velocity and maximum angular velocity; S45, selecting a linear velocity and an angular velocity in a feasible new velocity space, combining them and using them as the motion state of the robot, and predicting the predicted trajectory of the robot in the next period of time; S46. Calculate the braking distance between the robot and the nearest obstacle at each trajectory point of each predicted trajectory in sequence. The calculation formula is: In the above formula, and They represent the braking distances of the robot at time t and time t-1 respectively, and They represent the linear velocity of the robot at time t and time t-1 respectively, a represents the acceleration of the robot, To control the cycle, the loop is terminated when v≤0; S47, sequentially determining whether there is a trajectory point on each predicted trajectory where the braking distance between the robot and the nearest obstacle is greater than or equal to the actual distance; If so, the predicted trajectory is eliminated; If not, mark the predicted trajectory as a candidate trajectory; S48. Evaluate the candidate trajectories through the evaluation model, and select the candidate trajectory with the highest evaluation score as the temporary trajectory for the robot to move.

5. The path planning method according to claim 4, characterized in that: In step S44, the specific method of scaling the maximum linear velocity and maximum angular velocity of the robot driving wheels according to the obstacle characteristic data is: obtaining the obstacle characteristic data and updating the maximum linear velocity and maximum angular velocity of the robot driving wheels. The updating formula is: in, and Represent the maximum linear velocity and maximum angular velocity of the updated robot respectively. For large obstacles, the excitation factor <1, for small obstacles, the excitation factor >

1.

6. The path planning method according to claim 4, characterized in that: In step S48, the expression of the evaluation model is: in, represents the evaluation score of the evaluation model, and represents the linear velocity and angular velocity that the robot can move. is the heading angle evaluation item, which quantifies the deviation between the candidate trajectory direction and the target point orientation. Indicates the minimum Euclidean distance between the trajectory point and the nearest obstacle. The larger the distance, the higher the safety margin. It is a motion efficiency evaluation item. Reflects the linear velocity amplitude, used to improve motion efficiency, parameters 、 and are the first, second and third weight coefficients respectively, It is a nonlinear adjustment factor used to balance the contribution of each sub-item. Size sensitivity factor .

7. The path planning method according to claim 4, characterized in that: In step S43, the expression of the feasible speed space is: in, in, represents the feasible velocity space, represents the speed constraint based on the physical limits of linear and angular velocity of the drive system's mechanical performance. represents the speed constraint limited by the maximum acceleration and deceleration of the drive system, Represents the safe braking speed constraint based on the minimum distance between the robot's moving trajectory point and the obstacle, and represents the linear velocity and angular velocity that the robot can move. and are the minimum and maximum linear speeds of the robot’s driving wheels, and are the minimum and maximum angular velocities of the driving wheel, and are the linear velocity and angular velocity of the robot in the current state, and are the maximum linear acceleration and maximum angular acceleration of the robot, To control the cycle, is the shortest distance between the robot and the obstacle.

Images