Path planning method based on target characteristics

Abstract

The application relates to the technical field of path planning, in particular to a path planning method based on target characteristics; by adopting a window shrinkage and a strengthened path cost for a large obstacle, motion instability caused by steering mutation is effectively inhibited; a small obstacle is relaxed in constraint to improve local obstacle avoidance flexibility. Newly added brake safety verification ensures no sudden stop risk under sudden obstacles, and experimental data show that the scheme synchronously optimizes path length, task time, path smoothness and safety in a complex scene, meets the core needs of efficient and smooth operation of a commercial scrubber, adopts a shrinkage strategy for a large obstacle, relaxes constraint and reduces path penalty weight for a small obstacle, and allows more aggressive small-range maneuvering, the mechanism significantly improves scene adaptability; the technical problems of non-smooth trajectory, path detour and insufficient brake safety when a scrubber adopts a traditional DWA as a local algorithm for path planning are solved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of path planning, in particular to a path planning method based on target characteristics. BACKGROUND

[0002] With the construction of smart cities, commercial floor cleaning robots have gradually become an important tool in the modern cleaning field. The navigation of floor cleaning robots usually consists of positioning, mapping, perception and path planning. The planning can be divided into local path planning and global path planning. The global path planning generates a collision-free path from the starting point to the ending point according to the established map. The local path planning updates the path in real time according to the sensor information and handles the increased obstacles on the map. The dynamic window algorithm (DWA) strictly obeys the speed, acceleration and turning ability that the real robot can reach in planar motion, so it is widely used in local path planning.

[0003] However, in the working scene of the floor cleaning robot, the traditional DWA algorithm still has the following shortcomings: firstly, the fixed parameterization strategy of the traditional algorithm ignores the differences in physical characteristics of obstacles, which may cause path jitter when the robot faces large obstacles (such as shelves and walls) due to sudden changes in turning angle, affecting the stability of robot motion; secondly, in a dense mixed obstacle scene system, the fixed size of the dynamic window may cause path redundancy and detours or increase energy consumption, affecting cleaning coverage efficiency; thirdly, the brake safety verification is not embedded in the trajectory generation link, which may cause sudden stop or path oscillation due to improper speed planning when sudden obstacles intervene, threatening the safety of floor cleaning equipment and pedestrians. SUMMARY

[0004] In view of the deficiencies of the prior art, the present application provides a path planning method based on target characteristics, which solves the technical problems of trajectory non-smoothness, path detour and brake safety deficiency when the floor cleaning robot uses the traditional DWA as a local algorithm for path planning.

[0005] To solve the above technical problems, the present application provides the following technical scheme: a path planning method based on target characteristics, which specifically includes the following steps:

[0006] Map construction: 3D point cloud of the working scene is obtained in real time by laser radar, 3D point cloud within a predetermined range is intercepted and 2D grid map is constructed by radius filtering;

[0007] Positioning and map updating: the coordinates of the starting point of the robot on the 2D grid map are obtained, and the cost map is generated and updated in real time according to the 3D point cloud obtained by the radar in real time and the 2D grid map;

[0008] Obstacle classification: based on the logical threshold and the grid cost value of the 2D grid map, compared with the inflation area cost threshold of the corresponding cost map, to identify, classify and pre-store the obstacles in the 2D grid map to obtain obstacle characteristic data;

[0009] Path planning: planning the global path of the robot from the starting point to the target point on the 2D grid map, and planning the temporary trajectory of the robot movement in real time based on the obstacle characteristic data and the DWA algorithm;

[0010] Target point judgment: judging whether the robot reaches the target point;

[0011] If yes, end;

[0012] If no, return to positioning and map updating.

[0013] As preferred, in the map construction, the following steps are specifically included:

[0014] S21, collecting 3D point cloud of the work scene based on the laser radar;

[0015] S22, intercepting the 3D point cloud data within the height value range in the range of interest in the z-axis direction;

[0016] S23, using the radius filtering method to remove the dynamic outliers in the intercepted 3D point cloud data to obtain the 2D grid map.

[0017] As preferred, in the obstacle classification, the method of identifying and classifying the obstacles in the 2D grid map is specifically as follows: if the grid cost value exceeds the inflation area cost threshold of the cost map, it is determined that the grid is an obstacle, otherwise it is a non-obstacle; when the number of continuous grids occupied by the obstacle exceeds the logical threshold of the size of the obstacle, it is determined to be a large obstacle, otherwise it is a small obstacle.

[0018] As preferred, in the path planning, the following steps are specifically included:

[0019] S41, planning the global path of the robot from the starting point to the target point;

[0020] S42, using a four-wheel two-drive differential motion model to construct the motion model of the robot in the global coordinate system, and the expression of the motion model is:

[0021]

[0022] In the above formula, and are the instantaneous velocity components of the robot mass center in the x-axis and y-axis directions of the global coordinate system, is the robot orientation, that is, the angle between the direction of the robot mass center to the front of the vehicle and the x-axis in the global coordinate system, is a rate of change of the heading angle with respect to time, i.e. an angular velocity of the robot about an axis perpendicular to a plane on which the global coordinate system lies;

[0023] S43, sampling the velocity and the angular velocity according to the current velocity and the angular velocity of the robot to obtain a feasible velocity space;

[0024] S44, scaling the maximum linear velocity and the maximum angular velocity of the drive wheel of the robot according to the obstacle characteristic data, and regenerating a new feasible velocity space according to the scaled maximum linear velocity and the maximum angular velocity;

[0025] S45, selecting the linear velocity and the angular velocity in the new feasible velocity space to combine and serve as a motion state of the robot, and predicting a predicted trajectory of the robot in the next period of time;

[0026] S46, sequentially calculating the braking distance of the robot from the nearest obstacle at each trajectory point of each predicted trajectory, and the calculation formula is:

[0027]

[0028] In the above formula, and respectively represent the braking distance of the robot at time t and time t-1, and respectively represent the linear velocity of the robot at time t and time t-1, and a represents the acceleration of the robot, is a control period, and the loop is terminated when v≤0;

[0029] S47, sequentially determining whether there is a trajectory point on each predicted trajectory at which the braking distance of the robot from the nearest obstacle is greater than or equal to the actual distance;

[0030] If yes, the predicted trajectory is eliminated;

[0031] If no, the predicted trajectory is marked as a candidate trajectory;

[0032] S48, evaluating the candidate trajectory by an evaluation model, and selecting the candidate trajectory with the highest evaluation score as a temporary trajectory of the robot.

[0033] As a preferred, in step S44, the specific method of scaling the maximum linear velocity and the maximum angular velocity of the drive wheel of the robot according to the obstacle characteristic data is: obtaining the obstacle characteristic data, and updating the maximum linear velocity and the maximum angular velocity of the drive wheel of the robot, and the update formula is:

[0034]

[0035] wherein, and These represent the updated maximum linear velocity and maximum angular velocity of the robot, respectively. For large obstacles, the excitation factor is... <1, for small obstacles, makes the excitation factor >1.

[0036] Preferably, in step S48, the expression for the evaluation model is:

[0037]

[0038] in, This represents the evaluation score of the evaluation model. and This represents the possible linear and angular velocities of the robot as it moves. For the heading angle evaluation item, it quantifies the deviation between the orientation of the candidate trajectory and the azimuth of the target point. This represents the minimum Euclidean distance between the trajectory point and the nearest obstacle. A larger distance indicates a higher safety margin. (Motion efficiency evaluation item) Reflects the amplitude of linear velocity, used to improve motion efficiency; parameter , and These are the first, second, and third weighting coefficients, respectively. This is a non-linear adjustment factor used to balance the contributions of each sub-item. As a size-sensitive factor, for large obstacles, >1, for small obstacles <1.

[0039] Preferably, in step S43, the feasible expression for the velocity space is:

[0040]

[0041] in,

[0042]

[0043]

[0044]

[0045] in, Represents the feasible velocity space. This represents the velocity constraints based on the physical limits of linear velocity and angular velocity according to the mechanical performance of the drive system. This indicates that the speed is constrained by the maximum acceleration and deceleration of the drive system. This represents the safe braking speed constraint based on the minimum distance between the robot's trajectory point and the obstacle. and a linear velocity and an angular velocity feasible when the robot moves, and are respectively a minimum linear velocity and a maximum linear velocity of a driving wheel of the robot, and are respectively a minimum angular velocity and a maximum angular velocity of the driving wheel, and are respectively a linear velocity and an angular velocity of the robot in a current state, and are respectively a maximum linear acceleration and a maximum angular acceleration of the robot, is a control period, is a shortest distance of the robot from an obstacle.

[0046] By means of the above technical solution, the application provides a path planning method based on target characteristics, and at least has the following beneficial effects:

[0047] 1. The application effectively suppresses motion instability caused by sudden turning by adopting a tightened window and a strengthened path cost for large obstacles; and the constraint is relaxed for small obstacles to improve local obstacle avoidance flexibility. The newly added brake safety verification ensures no sudden stop risk under sudden obstacles, and experimental data show that the scheme simultaneously optimizes path length, task time, path smoothness and safety in complex scenes, and meets the core needs of efficient and smooth operation of commercial scrubbers.

[0048] 2. The application realizes dynamic fine adjustment of motion parameters by quantifying and grading physical characteristics of obstacles, adopts a tightening strategy for large obstacles: scales the maximum linear velocity and angular velocity by an excitation factor η<1, limits the motion range of the robot in single-step decision-making, and avoids path oscillation caused by inertia; at the same time, a size-sensitive factor is set in the evaluation function to strengthen the punishment weight of the heading deviation and the distance from the obstacle, forcing the robot to plan a smooth detour path in advance, and for small obstacles, the constraint is relaxed and the path punishment weight is reduced, allowing more aggressive small-range maneuvering. This mechanism significantly improves scene adaptability, reduces path redundancy in a dense mixed obstacle environment, improves obstacle avoidance efficiency, and reduces the standard deviation of angular velocity fluctuation by 45%, thus fundamentally solving the problems of path detour and jitter caused by fixed parameters in traditional DWA.

[0049] 3、The application innovatively embeds a brake safety verification module based on numerical integration in the trajectory prediction generation link, calculates the dynamic braking distance of each point of the predicted trajectory in real time, compares it with the actual obstacle distance, and pre-screens the predicted trajectory that has a collision risk, and the module fully considers the high inertia characteristics of the scrubber, ensures that the robot can slow down and stop within a safe distance when an obstacle suddenly intervenes, experiments show that the improved algorithm stably maintains the minimum obstacle distance at 0.17±0.01m safety threshold, and the collision times are zero, while the traditional DWA occurs collision due to the lack of this mechanism, this design avoids the sudden stop or path oscillation caused by improper speed planning from the front end of the control chain, and significantly improves the safety of the equipment and pedestrians. BRIEF DESCRIPTION OF DRAWINGS

[0050] The drawings described herein are used to provide further understanding of the present application, and form a part of the present application, the illustrative embodiments of the present application and the description thereof are used to explain the present application, and do not constitute improper limitation on the present application. In the drawings:

[0051] Figure 1 The flowchart of the path planning method based on the target characteristics of the application;

[0052] Figure 2 Three kinds of obstacle electronic maps for simulation experiments;

[0053] Figure 3 Actual experimental scene and corresponding 3D point and 2D map;

[0054] Figure 4 The schematic diagram of the motion model of the four-wheel two-drive differential car of the application;

[0055] Figure 5 The trajectory schematic diagram adopted by the car of the application;

[0056] Figure 6 The obstacle judgment schematic diagram of the application;

[0057] Figure 7 The distance judgment schematic diagram of the application;

[0058] Figure 8 The path planning diagram of two kinds of algorithms under scene 1;

[0059] Figure 9 The minimum distance change diagram of obstacles of two kinds of algorithms under scene 1;

[0060] Figure 10 The heading rate change diagram of two kinds of algorithms under scene 1;

[0061] Figure 11 The path curvature change diagram of two kinds of algorithms under scene 1;

[0062] Figure 12For the path curvature rate graph of two algorithms under scenario 1

[0063] Figure 13 For the simulation related graph of the traditional algorithm under scenario 2

[0064] Figure 14 For the simulation related graph of the improved algorithm under scenario 2

[0065] Figure 15 For the simulation related graph of the traditional algorithm under scenario 3

[0066] Figure 16 For the simulation related graph of the improved algorithm under scenario 3

[0067] Figure 17 For the Rviz graph of the traditional DWA algorithm

[0068] Figure 18 For the Rviz graph of the improved DWA algorithm of the application DETAILED DESCRIPTION

[0069] In order to make the above-mentioned purposes, features and advantages of the application more obvious and easy to understand, the application will be further described in detail below in combination with the drawings and specific embodiments. The realization process of how to apply technical means to solve technical problems and achieve technical effects can be fully understood and implemented.

[0070] The application aims at the problems of the traditional dynamic window algorithm (DWA) in the commercial floor cleaning machine operation scene, such as non-smooth trajectory, path detour and insufficient braking safety, etc. Based on the target characteristics, the obstacle is classified and avoided, and the dynamic window is combined to provide a path planning method based on target characteristics. The application first establishes a comparison standard of obstacle size and robot body size to form a quantitative classification criterion: for large obstacles, the dynamic window parameter is tightened and the path cost weight is strengthened to suppress the sudden turning and shaking; for small obstacles, the window constraint is relaxed and the path penalty is reduced to improve the obstacle avoidance flexibility. A braking safety margin verification module based on numerical integration is introduced in front of the trajectory evaluation layer to pre-screen the candidate trajectories with insufficient braking distance to avoid control instability caused by sudden speed change. Then, a "obstacle size-dynamic window-path cost" coupled optimization model is constructed to realize the adaptive mapping of environmental characteristics and motion strategy. Finally, to verify the effectiveness of the algorithm, a virtual scene with multiple obstacles is set up, and the path length, angular velocity standard deviation, curvature standard deviation, shortest distance between robot and obstacle, collision times and task time are used as evaluation indexes for simulation simulation. The differential chassis prototype based on the ROS platform is tested in real scene. The simulation and experimental results show that the improved algorithm is better than the traditional DWA algorithm in terms of time, path length and path smoothness.

[0071] 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 in the periphery, and small obstacles are concentrated in the middle of the scene and large obstacles are scattered around. Therefore, the three scenarios are analyzed respectively as shown in Figure 2 , where black represents obstacles, red dots represent target points, blue lines represent paths, and green fan-shaped windows represent velocity spaces;

[0072] In actual operation, since the working scene of a commercial floor cleaning machine usually has both large obstacles and small obstacles, an obstacle scene as shown in Figure 3 is created. In this scene, the stool is regarded as a small obstacle, while the table, wall, and side-by-side chairs are identified as large obstacles to meet the requirements of large and small obstacles in the scene.

[0073] When the floor cleaning robot moves, 3D laser point cloud information of the working scene can be collected using a Riegl C16 3D laser radar to obtain three-dimensional spatial point data through 3D SLAM technology of laser point cloud and three-dimensional laser sensor, and then the pose is estimated through scanning matching between adjacent point cloud frames. Further, a complete 3D point cloud is established by using loop detection and graph optimization technology, and then 3D point cloud data within the height range of interest in the z-axis direction is intercepted, and further dynamic outliers in the 3D point cloud data are removed using a radius filtering method to obtain a 2D grid map as shown in Figure 4 , where black represents obstacles. As can be seen from the figure, the obstacles in the processed 2D grid map are clear and can be used for path planning. In the initial use, a global 2D grid map is first established by the robot radar. In the subsequent working process of the robot, the 3D point cloud is obtained in real time by the radar configured on the robot to modify the 2D grid map to obtain a cost map. The initial 2D grid map and the real-time updated cost map are retained in the internal system of the robot, so that the newly added temporary obstacles can be obtained and updated on the cost map.

[0074] Positioning and map updating: the position coordinates of the robot on the 2D grid map are obtained, which are the coordinates of the starting point. Common algorithms include SLAM algorithms, and the cost map is generated and updated in real time by the 3D point cloud obtained in real time by the radar and the 2D grid map as the robot moves.

[0075] Obstacle classification: based on the inflation region cost threshold of the cost map corresponding to the 2D grid map, the grid cost value in the 2D grid map is compared, if the grid cost value exceeds the inflation region cost threshold of the cost map, the grid is determined as an obstacle, otherwise, it is a non-obstacle, when the number of continuous grids occupied by the obstacle exceeds the logical threshold of the size of the obstacle, it is determined as a large obstacle, otherwise, it is a small obstacle, and the classified obstacles are stored;

[0076] Path planning step: firstly, RRT* algorithm is adopted for global path planning, the core idea of which 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 finally obtained path tends to be optimal in cost, RRT* algorithm constructs a rapidly expanding tree by randomly sampling in the state space and gradually expanding from the existing nodes to the random sampling points, each time sampling, the algorithm finds the node closest to the sampling point in the current tree, then extends a small distance from the node to the sampling point to generate a new node, if the path is collision-free, the new node is added to the tree and connected to the nearest existing node, thereby obtaining a global path.

[0077] Then, in order to convert the position control into the speed control by using the DWA algorithm, the motion model of the robot needs to be analyzed, the kinematic model of the robot of the application is shown in Figure 4 , a four-wheel two-drive differential motion model is adopted, it is assumed that the displacement of the car is very small within the sampling time, and the moving track is treated as a straight line, at this time, the mathematical expression of the motion model is:

[0078]

[0079] In the above formula, and are the instantaneous velocity components of the robot centroid in the x-axis and y-axis directions of the global coordinate system, is the heading of the robot, that is, the angle between the direction of the robot centroid to the car 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.

[0080] 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 the feasible speed sampling space is calculated, the constraint conditions required in the sampling process are: the speed constraint based on the physical limit of the linear speed and angular speed of the drive system , the speed constraint limited by the maximum acceleration and deceleration of the drive system , the speed constraint of safety braking based on the minimum distance between the trajectory point and the obstacle , and the expressions of the three are:

[0081]

[0082]

[0083]

[0084] wherein, and denote the feasible linear and angular velocities of the robot, and are the minimum and maximum linear velocities of the robot's drive wheels, and are the minimum and maximum angular velocities of the drive wheels, and are the linear and angular velocities of the robot in the current state, and are the maximum linear and angular accelerations of the robot, is the control period, is the shortest distance of the robot from the obstacle.

[0085] The feasible velocity space of the car can be represented as: .

[0086] Finally, the feasible velocity and angular velocity within the velocity space are combined to form the motion state of the robot, and the motion trajectory is predicted for a certain simulation time. The sampled trajectory is shown in Figure 5 Further, after obtaining the sampled trajectory of the car, the predicted path needs to be evaluated by an evaluation function, and the state of the car corresponding to the path with the highest evaluation score is selected as the next running state of the robot. The expression of the evaluation function is as follows:

[0087]

[0088] wherein, denotes the evaluation score of the evaluation model, and denote the feasible linear and angular velocities of the robot, is the heading angle evaluation term, which quantifies the deviation of the candidate trajectory from the target point, denotes the minimum Euclidean distance between the trajectory point and the nearest obstacle, and the greater the distance, the higher the safety margin, reflects the linear velocity amplitude and is used to improve the motion efficiency, and the parameters , and are the first, second and third weight coefficients, respectively, is a nonlinear adjustment factor, used to balance the contribution of each subterm.

[0089] Due to the complex dynamic situation of the scrubber operation scene, if only relying on the RRT* algorithm, the local obstacle avoidance effect is insufficient, therefore, the DWA algorithm is used to avoid the temporary dynamic obstacle, and when the trolley moves, the obstacles within a certain range around the trolley are judged from the obtained 2D grid map.

[0090] In the process of path planning, when the traditional DWA algorithm performs safety evaluation on the obstacle, the traditional DWA algorithm does not consider the type of the obstacle, and establishes an obstacle table for all obstacles on the map, so as to perform brake safety judgment on the assumed robot state.

[0091] However, in the actual movement process, different sizes of obstacles have different effects on the trolley, and due to the large inertia of the scrubber itself, the insufficient brake safety distance may endanger the safety of the scrubber itself and pedestrians.

[0092] Therefore, the traditional DWA algorithm is improved in the present application, first, the obstacles are classified as Figure 6 , a conservative strategy is implemented for large obstacles, that is, the dynamic window parameter range is narrowed and the path cost weight is increased, 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 speed and maximum angular speed of the robot movement are enlarged, to ensure that the robot quickly reacts and bypasses when facing small obstacles, and the movement ability is improved, the update formula of the maximum linear speed and maximum angular speed of the robot driving wheel is:

[0093]

[0094] wherein, and respectively represent the updated maximum linear speed and maximum angular speed of the robot, for large obstacles, the excitation factor <1, which limits the movement ability of the robot, and for small obstacles, the excitation factor >1.

[0095] Then, the maximum linear speed and maximum angular speed of the updated robot are substituted into the formula in the foregoing to regenerate a new speed space, and the DWA algorithm is used to generate a predicted trajectory according to the new speed space, and numerical integration is used to realize trajectory safety prediction: assuming that the linear speed of the trolley is v and the acceleration is a, both v and a are discrete sampling values, then the iteration formula of the brake distance of the trolley from the nearest obstacle is:

[0096]

[0097] In the above formula, and respectively represent the braking distance of the robot at time t and time t-1, and respectively represent the linear velocity of the robot at time t and time t-1, and a represents the acceleration of the robot, is a control period, when v≤0, the loop is terminated, in a dynamic environment, the movement and change of the obstacle are unpredictable, the braking distance is calculated in advance, the current state can be adjusted more safely and in real time, the robot is prevented from suddenly changing speed or path, and thus the instability of control is reduced.

[0098] Then, whether there is a trajectory point on each predicted trajectory at which the braking distance between the robot and the nearest obstacle is greater than or equal to the actual distance is judged in sequence, if yes, the predicted trajectory is eliminated, and if no, the predicted trajectory is marked as a candidate trajectory.

[0099] Finally, the candidate trajectory is transmitted to an evaluation module, and a size-sensitive factor The evaluation function in the traditional DWA algorithm is improved, for a large obstacle, >1, for a small obstacle <1, then the expression of the improved evaluation function is:

[0100]

[0101] Finally, whether the robot reaches the target point is judged, if yes, the navigation is ended, and if no, positioning and map updating are returned, and the cycle is repeatedly performed.

[0102] Verification example one: DWA algorithm simulation comparison

[0103] In a random obstacle distribution scene, a start point is set, a target point is set, and the robot starts planning from the start point. From Figure 8 It can be seen from that the traditional DWA algorithm has obvious turning points when planning the path, the safety distance from the obstacle is redundant, and the obstacle avoidance fails; while the path generated by the improved algorithm has significantly improved smoothness, and can achieve safe bypassing of near obstacles. Meet the requirements of the scrubber operation. From Figure 9 It can be seen from that the traditional algorithm collides, while the improved algorithm maintains a collision safety threshold of 0.17±0.01 while improving the compactness of the path, and the experimental results verify the balance optimization of the improved algorithm between safety and path efficiency.

[0104] To quantitatively evaluate the smoothness of the path, the present application constructs a dual judgment model based on the curvature standard deviation and the angular velocity standard deviation. The angular velocity standard deviation reflects the rate of change of the heading (such as Figure 9The dynamic stability of the path (as shown in the figure) is determined by the curvature, which is the rate of rotation of the tangent direction of the path about the arc length, reflecting the local bending degree of the path (R is the radius of curvature); the standard deviation of curvature reflects the fluctuation of curvature.

[0105] Figure 10 The rate of change of the course along the path. Figure 11 and Figure 12 These are a path curvature variation graph and a path curvature rate of change graph, respectively. Figures 10-12 It can be seen that both the traditional algorithm and the improved algorithm exhibit peak angular velocity in the initial stage (0-5 seconds) and have a large path curvature. This is essentially the active turning adjustment requirement of 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 problem.

[0106] The improved algorithm adjusts the parameters of the dynamic window and the path cost weights based on the obstacle size. When facing large obstacles ( Figure 11 Traditional algorithms, with graph indices ranging from 250 to 300, limit the range of angular velocities the robot can choose in a single-step decision, constraining the turning amplitude of candidate paths. Dynamic adjustment of path costs enables the algorithm to actively avoid high-curvature paths, simultaneously reducing both the mean and extreme values ​​of curvature. Experimental metrics are shown in Table 1.

[0107] Table 1 Comparison of various indicators of the DWA algorithm before and after improvement

[0108]

[0109] As can be seen from Table 1, the improved algorithm reduces path time, path length, standard deviation of angular velocity, and standard deviation of curvature in this scenario. The performance indicators show that the improved algorithm outperforms the original algorithm.

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

[0111] Table 2 Comparison of various metrics of the DWA algorithm before and after improvement (Scenario 2)

[0112]

[0113] Simulation diagrams related to traditional algorithms in scenario 3 are as follows: Figure 15 The improved algorithm simulation diagram is as follows: Figure 16 The performance indicators are shown in Table 3.

[0114] Table 3 Comparison of various indicators of the DWA algorithm before and after improvement (Scenario 3)

[0115]

[0116] As can be seen from Table 3, in the case of reaching the target point, the minimum distance of the traditional algorithm is lower than the safety distance threshold, and collision may occur, while the improved algorithm maintains the safety distance threshold, and the position distance is smaller, and the path length, path time, angular velocity standard deviation and curvature standard deviation are reduced.

[0117] Verification example two: actual scene experiment comparison

[0118] Since the commercial floor cleaning machine usually uses a four-wheel two-drive differential car as a mobile platform, in order to verify whether the improved algorithm can be used for obstacle avoidance of the floor cleaning machine, the four-wheel two-drive car prototype equipped with a C16 type 3D laser radar will be used to perform autonomous obstacle avoidance experiments in the scene as shown in Figure 2 .

[0119] The experimental RVIZ diagram is shown in Figure 17 and Figure 18 . Figure 17 is the RVIZ diagram of the traditional DWA algorithm, Figure 18 is the RVIZ diagram of the improved DWA algorithm, RVIZ is a visualization software, and the quantitative path of the car from the starting point to the end point can be clearly seen. The blue line in the figure is the integral path actually traveled by the robot, the green line is the feasible path when the DWA is sampled, and the green frame is the robot contour. Black is the obstacle, and gray is the inflation area of the cost map. Near the gray area, the path cost will be increased to make the robot away from the obstacle for path planning. Table 4 is the performance index of the obstacle avoidance experiment of the two algorithms.

[0120] Table 4 Performance index of DWA algorithm before and after improvement

[0121]

[0122] From Figure 18 and Table 4, it can be seen that due to the dynamic adjustment of the cost of the improved algorithm, the path length, the minimum distance from the obstacle under the threshold condition, the angular velocity standard deviation, the curvature standard deviation are reduced, and the path time is greatly shortened, which improves the path planning efficiency of the floor cleaning machine.

[0123] Those skilled in the art can understand that all or part of the steps of the above-mentioned embodiment methods can be completed by programs instructing related hardware, therefore, the present application can adopt a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application can adopt the form of a computer program product implemented on one or more computer usable storage media (including but not limited to disk memory, CD-ROM, optical memory, etc.) containing computer usable program codes.

[0124] Each of the embodiments in the specification is described in a progressive manner, and each embodiment focuses on the difference from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the above embodiments, the description is relatively simple because it is basically similar to the method embodiment, and the relevant part can be referred to the part of the method embodiment.

[0125] The above embodiments have introduced the present application in detail, and the principle and implementation of the present application are described by applying specific examples. The above embodiment is only used to help understand the method of the present application and its core idea. Meanwhile, for the general technical personnel in the field, the specific implementation and application range will be changed according to the idea of the present application. In summary, the content of the specification should not be understood as the limitation of the present application.

Claims

1. A path planning method based on target characteristics, characterized in that, The path planning method specifically includes the following steps: Map building: 3D point cloud of the work scene is acquired in real time by LiDAR, 3D point cloud within a predetermined range is extracted and a 2D raster map is built by radius filtering; Localization and map update: 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 obtained by radar in real time; Obstacle classification: Based on logical thresholds and the grid cost value of the 2D grid map, the obstacle in the 2D grid map is identified, classified, and pre-stored by comparing it with the expansion region cost threshold of the corresponding cost map to obtain obstacle characteristic data. The specific method for identifying and classifying obstacles in the 2D grid map is as follows: if the grid cost value exceeds the expansion region 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 an obstacle exceeds the logical threshold of the obstacle size, it is determined to be a large obstacle; otherwise, it is a small obstacle. Path planning: This involves planning the robot's global path from the starting point to the target point on a 2D grid map, and planning temporary trajectories for the robot's movement in real time based on obstacle characteristic data and the DWA algorithm. Specifically, this includes the following steps: S43. Based on the robot's current speed and angular velocity, sample the speed and angular velocity to obtain a feasible speed space; S44. Scale the maximum linear velocity and maximum angular velocity of the robot's drive wheels based on obstacle characteristic data. Then, regenerate a feasible new velocity space based on the scaled maximum linear velocity and maximum angular velocity. The update formula is: ； in, and These represent the updated maximum linear velocity and maximum angular velocity of the robot, respectively. For large obstacles, the excitation factor is... <1, for small obstacles, makes the excitation factor >1; S45. Select the linear velocity and angular velocity in the feasible new velocity space, combine them as the robot's motion state, and predict the robot's trajectory in the next time period. S46. Calculate the braking distance of the robot from the nearest obstacle at each trajectory point of each predicted trajectory in sequence. The calculation formula is as follows: ； In the above formula, and These represent the braking distances of the robot at time t and time t-1, respectively. and Let represent the linear velocities of the robot at time t and t-1, respectively, and let a represent the robot's acceleration. To control the cycle, the loop terminates when v ≤ 0; S47. Sequentially determine whether there are trajectory points 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, then discard the predicted trajectory; If not, then mark the predicted trajectory as a candidate trajectory; S48. Evaluate the candidate trajectories using the evaluation model, and select the candidate trajectory with the highest evaluation score as the temporary trajectory for the robot's movement. Target point determination: Determine whether the robot has reached the target point; If so, then the process ends; If not, return to location and map update.

2. The path planning method according to claim 1, characterized in that, Before step S43, the following steps are also included: S41. Plan the robot's global path from the starting point to the target point; S42. A motion model of the robot in the global coordinate system is constructed using a four-wheel, two-drive differential motion model. The expression of this motion model is: ； In the above formula, and Let be the instantaneous velocity components of the robot's center of mass in the x and y directions of the global coordinate system. It refers to the robot's orientation, specifically the angle between the robot's center of mass (from the front of the robot) and the x-axis in the global coordinate system. It is the rate of change of the heading angle over time, that is, the angular velocity of the robot about the axis perpendicular to the plane of the global coordinate system.

3. The path planning method according to claim 1, characterized in that, Map building specifically includes the following steps: S21. 3D point cloud based on lidar acquisition scenario; S22. Extract 3D point cloud data within the height value range of the area of ​​interest along the z-axis; S23. Use the radius filtering method to remove dynamic outliers from the captured 3D point cloud data to obtain a 2D raster map.

4. The path planning method according to claim 1, characterized in that, In step S48, the expression for the evaluation model is: ； in, This represents the evaluation score of the evaluation model. and This represents the possible linear and angular velocities of the robot as it moves. For the heading angle evaluation item, it quantifies the deviation between the orientation of the candidate trajectory and the azimuth of the target point. This represents the minimum Euclidean distance between the trajectory point and the nearest obstacle. A larger distance indicates a higher safety margin. (Motion efficiency evaluation item) Reflects the amplitude of linear velocity, used to improve motion efficiency; parameter , and These are the first, second, and third weighting coefficients, respectively. This is a non-linear adjustment factor used to balance the contributions of each sub-item. As a size-sensitive factor, for large obstacles, >1, for small obstacles <1.

5. The path planning method according to claim 1, characterized in that, In step S43, the feasible expression for the velocity space is: ； in, ； ； ； in, Represents the feasible velocity space. This represents the velocity constraints based on the physical limits of linear velocity and angular velocity according to the mechanical performance of the drive system. This indicates that the speed is constrained by the maximum acceleration and deceleration of the drive system. This represents the safe braking speed constraint based on the minimum distance between the robot's trajectory point and the obstacle. and This represents the possible linear and angular velocities of the robot as it moves. and These are the minimum and maximum linear velocities of the robot's drive wheels, respectively. and These are the minimum and maximum angular velocities of the drive wheels, respectively. and The linear velocity and angular velocity of the robot in the current state are... and These are the robot's maximum linear acceleration and maximum angular acceleration, respectively. To control the cycle, This represents the shortest distance between the robot and the obstacle.

Images