Obstacle avoidance planning method, system and device

By combining DWA and ORCA algorithms, particles are gradually defined and optimal particles are selected in feasible domains, the problems of collision and space waste in differential robot navigation are solved, and efficient multi-robot obstacle avoidance planning is achieved.

CN113960995BActive Publication Date: 2024-11-19JUXING TECH SHENZHEN CO LTD
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202010698971.3
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2020-07-20
Publication Date
2024-11-19
Estimated Expiration
2040-07-20

AI Technical Summary

Technical Problem

In the prior art, the DWA algorithm used by differential robots during navigation cannot effectively consider the coordination between multiple robots, resulting in collision and space waste, especially errors occurring in conversion speed and angular velocity.

Method used

Combining the DWA algorithm and the ORCA algorithm, we can gradually select the velocity and angular velocity to define particles, generate a score array based on map information and positioning information, and select the optimal particles in the feasible domain through the optimal collision avoidance algorithm, expand the robot's obstacle avoidance radius, and generate Multiple boundary lines, judge path errors and determine the optimal particles.

Benefits of technology

It effectively avoids collision problems caused by too large time interval, speed and angular velocity, retains the multi-machine planning performance of the ORCA algorithm, and adapts to the characteristics of differential drive robots, improving the performance of obstacle avoidance planning.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN113960995B_ABST
    Figure CN113960995B_ABST
Patent Text Reader

Abstract

The present invention discloses an obstacle avoidance planning method, system and device. The method comprises: taking a velocity and an angular velocity one by one to define a particle; then, according to the map information and positioning information, and in combination with the local obstacle avoidance dynamic window algorithm, determining a scoring array containing the particle, the velocity and the angular velocity; finally, according to the scoring array, and in combination with the optimal mutual collision avoidance algorithm, selecting the optimal particle in the feasible domain. A high-performance obstacle avoidance planning scheme is implemented, which solves the conversion problem of different velocity vectors, effectively avoids the large error between the real path and the actual path due to the large time interval, speed and angular velocity, and the collision problem that may arise. While retaining the good multi-machine planning performance based on the ORCA algorithm, it also adapts to the characteristics of the differential drive robot.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of robotics technology, and in particular to an obstacle avoidance planning method, system and device. Background Art

[0002] In the prior art, when a differential robot is navigating, an A-star or Dijsktra (shortest path) algorithm is often used to generate a global path, and then a DWA (dynamic window) algorithm is used for local planning and obstacle avoidance. Specifically, an interval is opened on the left and right of the current speed and angular velocity, and the two intervals are sampled separately to obtain a pair of (v, w) arrays, and a curve is generated using this array. Thus, multiple curves can be obtained through multiple samplings, and then the corresponding loss function is calculated for each curve and the (v, w) array of the curve with the smallest loss is selected and output to the control module. However, the DWA algorithm is only a planning algorithm for a single machine, and the algorithm does not consider the movement of other robots, which will lead to two undesirable situations: first, if other robots move too fast after planning, a collision will occur; second, if multiple robots cannot give way to each other in one place, they will be stuck in the same place at the same time.

[0003] In order to enable multiple robots to coordinate with each other, a feasible solution is to let all robots avoid obstacles by sharing half of the responsibility for obstacle avoidance. For example, the ORCA (Optimal Mutual Collision Avoidance) algorithm is used. The ORCA algorithm is planned in the speed domain. When a robot faces a robot and finds after calculation that it will collide with it after t time, it calculates how much speed needs to be reduced to avoid the collision and assume half of the responsibility. Similarly, the other robot also calculates and assumes half of the responsibility in the same way. Finally, the two robots avoid collision because they each assume half of the responsibility. In addition, since the ORCA algorithm uses a linear programming method to obtain a linear optimal path, when they are blocked, because the feasible domain of one robot is behind another robot, the robot will give way. At the same time, because other robots find that there is a vacant position, they will fill it or also vacate the position. As a result, the three robots can pass through in turn. However, it should be noted that the output of the ORCA algorithm is an array (vx, vy) that only contains the x-direction velocity and the y-direction velocity, while the differential drive robot requires the output to be (v, w), that is, a linear velocity and an angular velocity, which requires a conversion from (vx, vy) to (v, w). The existing conversion method will bring a delta error. For this, the common practice is to add delta to the radius of the abstract robot of the ORCA algorithm. However, this processing method not only leads to a waste of space, but also because the radius of the abstract robot changes under different (vx, vy) when performing linear planning, the feasible domain will also change all the time. This leads to a situation where the path generated by the ideal (vx, vy) can be well avoided, but the path actually generated by (v, w) will collide. In view of the above technical defects, one solution is to adopt a method combining the DWA algorithm and the ORCA algorithm. For example, the DWA algorithm is first used to generate the optimal path as the reference speed, and then the ORCA algorithm is used for linear planning to obtain the optimal one. However, this method may also cause the above-mentioned delta to be too large and cause a collision when the time interval dt, linear velocity v, and angular velocity w are too large.

[0004] In summary, in the prior art, there is no technical solution that can well combine the DWA algorithm and the ORCA algorithm to perform optimal obstacle avoidance path planning. Summary of the invention

[0005] In order to solve the above technical defects in the prior art, the present invention proposes an obstacle avoidance planning method, which includes:

[0006] Define a particle by taking a velocity and angular velocity one by one;

[0007] Determine a scoring array including the particle, the velocity, and the angular velocity according to the map information and the positioning information and in combination with a local obstacle avoidance dynamic window algorithm;

[0008] According to the scoring array, the best particle is selected in the feasible domain in combination with the best mutual collision avoidance algorithm.

[0009] Optionally, the step of successively taking a velocity and an angular velocity to define a particle includes:

[0010] In combination with the local obstacle avoidance dynamic window algorithm, determine a speed interval corresponding to the current speed and an angular velocity interval corresponding to the current angular velocity;

[0011] In the speed interval and the angular velocity interval, a speed value and an angular velocity value are successively taken as the attribute of the particle at preset time intervals.

[0012] Optionally, selecting the best particle in the feasible domain according to the scoring array and in combination with an optimal mutual collision avoidance algorithm comprises:

[0013] The obstacle avoidance radius of the robot is expanded according to the particles, the speed and the angular velocity, and a plurality of boundary lines are generated in combination with the optimal mutual collision avoidance algorithm.

[0014] Optionally, after generating a plurality of boundary lines in combination with the optimal mutual collision avoidance algorithm, the method further comprises:

[0015] Determine successively at the time interval whether the velocity array corresponding to the particle is within the boundary line;

[0016] If the speed array is within the boundary line, it is determined whether the current path error is less than a preset error value.

[0017] Optionally, the determining whether the current path error is less than a preset error value includes:

[0018] If the path error is smaller than a preset error value, the magnitude relationship between the index number of the particle and the preset primary selection number is determined.

[0019] Optionally, the determining the magnitude relationship between the index number of the particle and a preset first-level selection number includes:

[0020] If the index number is less than the first-level selection number, six loss items are determined, wherein the six loss items include the Euclidean distance to the nearest global planning point, the heading angle difference to the nearest global planning point, the Euclidean distance to the preset point after the nearest global planning point, the heading angle difference to the preset point after the nearest global planning point, the distance to the nearest obstacle on the curve, and the penalty item of angular velocity;

[0021] The six loss items are linearly weighted as the overall loss, and the overall loss is saved in the loss array.

[0022] Optionally, the determining the magnitude relationship between the index number of the particle and a preset first-level selection number further includes:

[0023] If the index number is equal to the first-level selection number, the speed array corresponding to the minimum overall loss is determined in the loss array, and the particle corresponding to the speed array is used as the optimal particle.

[0024] Optionally, the determining the magnitude relationship between the index number of the particle and a preset first-level selection number further includes:

[0025] If the index number is greater than the primary selection number and the index number is less than the preset secondary selection number, then determine a velocity array whose path error is less than a preset error value and is within the boundary line, and use the particle corresponding to the velocity array as the optimal particle;

[0026] If the index number is greater than the secondary selection number, the speed array corresponding to the maximum score is determined according to the local obstacle avoidance dynamic window algorithm, and the particle corresponding to the speed array is used as the optimal particle.

[0027] The present invention also proposes an obstacle avoidance planning system, which comprises:

[0028] A local obstacle avoidance dynamic window planner is used to successively take a velocity and an angular velocity to define a particle, and determine a scoring array including the particle, the velocity, and the angular velocity according to the map information and the positioning information and in combination with the local obstacle avoidance dynamic window algorithm;

[0029] The optimal mutual collision avoidance planner is used to select the optimal particles in the feasible domain according to the score array and in combination with the optimal mutual collision avoidance algorithm.

[0030] Optionally, the system further includes an instant positioning and map construction module, a local cost map module, a global cost map module, a positioning information module and a cloud storage module, wherein:

[0031] The local cost map module is used to obtain local cost map data;

[0032] The global cost map module is used to obtain global cost map data;

[0033] The positioning information module is used to obtain positioning information data;

[0034] The cloud storage module is used to read the position and speed data of other robots;

[0035] The real-time positioning and map construction module is used to provide the local cost map data, the global cost map data and the positioning information data to the local obstacle avoidance dynamic window planner;

[0036] The local obstacle avoidance dynamic window planner is further used to generate the scoring array according to the local cost map data, the global cost map data and the positioning information data;

[0037] The optimal mutual collision avoidance planner is also used to output an optimal speed array based on the score array and the position speed data.

[0038] The present invention also proposes an obstacle avoidance planning device, which includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the computer program implements the steps of the obstacle avoidance planning method as described in any one of the above items when executed by the processor.

[0039] The obstacle avoidance planning method, system and device of the present invention are implemented to define a particle by taking a speed and an angular velocity one by one; then, according to the map information and positioning information, and in combination with the local obstacle avoidance dynamic window algorithm, a scoring array containing the particle, the speed and the angular velocity is determined; finally, according to the scoring array, and in combination with the optimal mutual collision avoidance algorithm, the optimal particle is selected in the feasible domain. A high-performance obstacle avoidance planning scheme is realized, which solves the conversion problem of different speed vectors, effectively avoids the excessive error caused by too large time interval, speed and angular velocity, and the collision problem that may arise. While retaining the good multi-machine planning performance based on the ORCA algorithm, it also adapts to the characteristics of the differential drive robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] The present invention will be further described below with reference to the accompanying drawings and embodiments, in which:

[0041] Figure 1 is a first flow chart of the obstacle avoidance planning method provided by an embodiment of the present invention;

[0042] Figure 2 is a first error schematic diagram of the obstacle avoidance planning method provided by an embodiment of the present invention;

[0043] Figure 3 is a second error schematic diagram of the obstacle avoidance planning method provided by an embodiment of the present invention;

[0044] Figure 4 It is a schematic diagram of a planner flow chart of an obstacle avoidance planning method provided in an embodiment of the present invention;

[0045] Figure 5 is a schematic diagram of a first module of an obstacle avoidance planning system provided by an embodiment of the present invention;

[0046] Figure 6 It is a schematic diagram of the second module of the obstacle avoidance planning system provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0047] It should be understood that the specific embodiments described herein are only used to explain the present invention, and are not used to limit the present invention.

[0048] In the subsequent description, the suffixes such as "module", "component" or "unit" used to represent elements are only used to facilitate the description of the present invention, and have no specific meanings. Therefore, "module", "component" or "unit" can be used in a mixed manner.

[0049] Embodiment 1

[0050] Figure 1 is a first flow chart of the obstacle avoidance planning method provided by an embodiment of the present invention. This embodiment proposes an obstacle avoidance planning method, the method comprising:

[0051] S1, successively taking a velocity and angular velocity to define a particle;

[0052] S2. Determine a scoring array including the particle, the velocity, and the angular velocity according to the map information and the positioning information and in combination with a local obstacle avoidance dynamic window algorithm;

[0053] S3. Selecting the best particle in the feasible domain according to the scoring array and in combination with the best mutual collision avoidance algorithm.

[0054] First, it should be noted that this embodiment takes into account that in the prior art, the output of the ORCA algorithm is an array (vx, vy) containing only the x-direction velocity and the y-direction velocity, while the differential drive robot requires the output to be (v, w), that is, a linear velocity and an angular velocity, which requires a conversion from (vx, vy) to (v, w). For details, see Figure 2 , which is a first error schematic diagram of the obstacle avoidance planning method provided by an embodiment of the present invention. In this figure, the straight line is an ideal path, and the curve is an actual path. Therefore, it can be seen that the existing conversion method will bring a delta error. In this regard, a common practice is to add the delta error to the radius of the abstract robot of the ORCA algorithm. However, this processing method not only leads to a waste of space, but also because the radius of the abstract robot changes under different (vx, vy) when performing linear planning, the feasible domain will also change all the time, which leads to a situation where the obstacle avoidance planning can be well performed under the path generated by the ideal (vx, vy), but the path actually generated by (v, w) will collide.

[0055] In order to solve the above technical problems, a method combining the DWA algorithm and the ORCA algorithm is proposed, but this method also has obvious technical defects. For example, the DWA algorithm is first used to generate the optimal path as the reference speed, and then the ORCA algorithm is used for linear planning to obtain the optimal value. However, this method will also cause the above-mentioned delta to be too large and cause a collision when the time interval dt, linear velocity v, and angular velocity w are too large. For details, see Figure 3 , which is a second error diagram of the obstacle avoidance planning method provided by an embodiment of the present invention. When the interval time dt=1s and the speed v=1.5m / s, the delta-w / 2 relationship diagram (that is, the delta error diagram) shown in the figure is obtained. In this error diagram, the horizontal coordinate is w / 2 (angular velocity / 2), and the vertical coordinate is delta, that is, the maximum difference between the path of vx, vy and the actual path taken by the differential drive robot. It can be seen that when w=pi, the delta error is about 0.26m. In the applicable scenario of warehousing and logistics robots, if the delta error of multiple robots is about 0.26m, a total error of 0.52m will be generated, which will have a relatively adverse effect on the robot's obstacle avoidance planning.

[0056] In this embodiment, considering that different speeds v and angular velocities w may cause the feasible domain based on the ORCA algorithm to change, this embodiment proposes a solution that combines the DWA algorithm and the ORCA algorithm that is suitable for differential drive robots. Specifically, first, a speed and an angular velocity are taken one by one to define a particle; then, according to the map information and positioning information, and in combination with the local obstacle avoidance dynamic window algorithm, a scoring array containing the particle, the speed, and the angular velocity is determined; finally, according to the scoring array, and in combination with the optimal mutual collision avoidance algorithm, the optimal particle is selected in the feasible domain. It can be seen that since the points brought into the ORCA algorithm in this solution are generated by the DWA algorithm, the feasible domain based on the ORCA algorithm will not change at each linear programming. While solving the above-mentioned (vx, vy) to (v, w) conversion problem, it effectively avoids the collision problem that may be caused by excessive delta error due to too large time interval dt, speed v, and angular velocity w. Further, while retaining the good multi-machine planning performance based on the ORCA algorithm, it also adapts to the characteristics of the differential drive robot.

[0057] In this embodiment, in order to implement the obstacle avoidance planning method, a planner processing solution is proposed. Figure 4, which is a schematic diagram of the planner module of the obstacle avoidance planning method provided by an embodiment of the present invention. Among them, the DWAPlanner planner module reads the local cost map local Map, the global cost map Global Plan and the positioning information Localization provided by the SLAM (simultaneous localization and mapping) module, and then generates an array containing the speed v, angular velocity w and various scores of the particles. Then, the array of the speed v, angular velocity w and various scores of the particles is used as the input item of the ORCA Planner planner. At the same time, the ORCAPlanner planner also reads the position speed and other information of other robots in the Cloud storage through shadows. Finally, the ORCA Planner planner outputs an optimal speed array (v, w).

[0058] Specifically, in this embodiment, firstly, based on the DWA algorithm, for the current robot, an interval is opened on the left and right of its velocity v and angular velocity w, that is, a number is taken on [v-dv, v+dv] and [w-dw, w+dw] as an attribute of the particle, and the corresponding trajectory score generated by the differential motion equation is calculated for each particle (v, w), and then, each score is input into the ORCA Planner along with the corresponding v and w. At the same time, the ORCA Planner receives the position and velocity information of each robot from the server. Finally, the ORCA Planner processes the above input information and outputs the optimal particle in the feasible domain.

[0059] The beneficial effect of this embodiment is that a particle is defined by taking a velocity and an angular velocity one by one; then, a scoring array containing the particle, the velocity, and the angular velocity is determined according to the map information and positioning information and in combination with the local obstacle avoidance dynamic window algorithm; finally, the optimal particle is selected in the feasible domain according to the scoring array and in combination with the optimal mutual collision avoidance algorithm. A high-performance obstacle avoidance planning scheme is implemented, which solves the conversion problem of different velocity vectors, effectively avoids the excessive delta error caused by the large time interval dt, velocity v, and angular velocity w, and the collision problem that may arise. While retaining the good multi-machine planning performance based on the ORCA algorithm, it also adapts to the characteristics of the differential drive robot.

[0060] Embodiment 2

[0061] Based on the above embodiment, in order to define a particle according to the speed and angular velocity and establish a corresponding particle index, in this embodiment:

[0062] First, combining the local obstacle avoidance dynamic window algorithm, determining a speed interval corresponding to the current speed and an angular velocity interval corresponding to the current angular velocity;

[0063] Then, within the speed interval and the angular velocity interval, a speed value and an angular velocity value are successively taken as the attribute of the particle at preset time intervals.

[0064] Specifically, for example, obtain the speed and angular velocity of the current robot, and let the speed and angular velocity be v and w respectively. Based on this, take v in the interval [v-dv, v+dv] at a fixed interval. Similarly, take w in the interval of w at a fixed interval. If the interval length of v is 10 and the time interval is 1, 10 numbers can be obtained. Similarly, if the interval length of w is 15 and the time interval is 1, 15 numbers can be obtained. Finally, a 10*15 velocity array (v, w) is obtained, and each value in the array is taken as the attribute of a particle.

[0065] Optionally, a velocity value and an angular velocity value are successively taken as the property of the particle at the same preset time interval, wherein the time interval is 0.1 s;

[0066] Optionally, a velocity value and an angular velocity value are successively taken as the attribute of the particle at different preset intervals.

[0067] The beneficial effect of this embodiment is that, through the local obstacle avoidance dynamic window algorithm, a speed interval corresponding to the current speed and an angular velocity interval corresponding to the current angular velocity are determined; within the speed interval and the angular velocity interval, a speed value and an angular velocity value are successively taken as the properties of the particle at preset intervals, respectively. This provides a particle basis and a particle index for subsequent optimal particle screening.

[0068] Embodiment 3

[0069] Based on the above embodiment, in order to select the best particle in the feasible domain according to the scoring array and in combination with the best mutual collision avoidance algorithm, in this embodiment:

[0070] The obstacle avoidance radius of the robot is expanded according to the particles, the speed and the angular velocity, and a plurality of boundary lines are generated in combination with the optimal mutual collision avoidance algorithm.

[0071] Specifically, in this embodiment, first, the particle index i=0 and the feasible particle number p_ok=0 are set, and then the radius of the abstract robot is expanded according to the following formula.

[0072]

[0073] Finally, multiple ORCA lines are generated based on the ORCA algorithm as the boundary lines of this embodiment.

[0074] In this embodiment, after generating multiple boundary lines in combination with the optimal mutual collision avoidance algorithm, it is then determined whether the velocity array corresponding to the particle is within the boundary line at the time interval; if the velocity array is within the boundary line, it is determined whether the current path error is less than a preset error value, wherein the path error is the error between the real path and the actual path. Figure 5 , which is a schematic diagram of the planner flow of the obstacle avoidance planning method provided by an embodiment of the present invention. After generating multiple ORCA lines, it is determined whether the terminal of the velocity array (v, w) after each time interval is within the above ORCA line, and whether the current path error delta is less than the preset error value delta_max. Among them, the end point after a time interval is calculated according to the following motion model:

[0075] x=x+vΔtcos(θ t )

[0076] y=y+vΔtsin(θ t )

[0077] θ t =θ t +wΔt

[0078] In this embodiment, it should be noted that the ORCA line is in the form of a point-oriented type, that is, it is composed of a starting point and a direction array, where LXP = |L||P|sin<L,P> ,It can be seen that for this direction array in the velocity domain, when LXP<0, it can be judged that the end point is within the ORCA line;

[0079] Optionally, in this embodiment, the delta_max is determined according to the error caused by the robot delay, that is, when the planning frequency of the robot is reduced, the delta_max is synchronously reduced.

[0080] In this embodiment, after determining whether the current path error is less than a preset error value, if the path error is less than the preset error value, the magnitude relationship between the index number i of the particle and the preset primary selection number n is determined. Specifically:

[0081] If the index number i is less than the first-level selection number n, six loss items cost are determined, wherein the six loss items include the Euclidean distance of the nearest global planning point, the heading angle difference with the nearest global planning point, the Euclidean distance with the preset point after the nearest global planning point, the heading angle difference with the preset point after the nearest global planning point, the distance to the nearest obstacle on the curve, and the penalty item of the angular velocity w; then, the six loss items are linearly weighted as the overall loss, and the overall loss is saved in the loss array;

[0082] If the index number i is equal to the first-level selection number n, then the speed array (v, w) corresponding to the minimum overall loss cost is determined in the loss array, and the particle corresponding to the speed array (v, w) is used as the optimal particle;

[0083] If the index number i is greater than the primary selection number n, and the index number i is less than the preset secondary selection number m, then the velocity array (v, w) whose path error is less than the preset error value and within the boundary line is determined, and the particle corresponding to the velocity array (v, w) is used as the optimal particle;

[0084] If the index number i is greater than the secondary selection number m, the speed array (v, w) corresponding to the maximum score is determined according to the local obstacle avoidance dynamic window algorithm, and the particle corresponding to the speed array (v, w) is used as the optimal particle.

[0085] Optionally, in this embodiment, the primary selection number n is smaller than the secondary selection number m;

[0086] Optionally, in this embodiment, by increasing n, or m, the calculation is made more comprehensive, but correspondingly, as more i values ​​are taken, the calculation speed will be slower each time. It should also be noted that increasing n requires more operations than increasing m.

[0087] Optionally, in this embodiment, if n, or m is too small, the ORCA optimal point may not be found due to the small number of i values ​​taken, in which case 0 will be output.

[0088] Optionally, in this embodiment, if n is too small, it will tend to find a suboptimal point, and if it is not found, 0 is outputted as well.

[0089] Optionally, in this embodiment, the loss function of the DWA algorithm is changed while the ORCA algorithm remains unchanged to achieve the same or better effect. For example, based on the loss terms of the orientation difference, distance difference, speed v, angular velocity w, and the distance of the curve from the obstacle to the nearest point of the global path considered in the above embodiment, the loss terms of speed v, angular velocity w, and the orientation difference and distance difference of the nth point after the nearest point are added;

[0090] Optionally, in this embodiment, instead of defining a particle by taking a velocity and an angular velocity one by one, the global path is directly Gaussian sampled and then particles are provided to the ORCA algorithm to achieve the same or better effect. It should be noted that this replacement scheme may generate more unnecessary particles, thereby increasing the amount of calculation, but since this replacement scheme is random sampling, when the number of samples is large, the acceptance rate of the ORCA algorithm will increase accordingly.

[0091] The beneficial effect of this embodiment is that by judging the size relationship between the index number i of the particle and the preset first-level selection number n and second-level selection number m, and determining the corresponding optimal particle output according to the judgment result, a high-performance obstacle avoidance planning scheme is implemented, which solves the conversion problem of different speed vectors, effectively avoids the excessive delta error caused by too large time interval dt, speed v, and angular velocity w, and the possible collision problem, while retaining the better multi-machine planning performance based on the ORCA algorithm, it also adapts to the characteristics of the differential drive robot.

[0092] Embodiment 4

[0093] Figure 5 1 is a schematic diagram of the first module of the obstacle avoidance planning system provided by an embodiment of the present invention. Based on the above embodiment, this embodiment proposes an obstacle avoidance planning system, which includes a local obstacle avoidance dynamic window planner 10 and an optimal mutual collision avoidance planner 20. Among them:

[0094] A local obstacle avoidance dynamic window planner 10 is used to successively take a velocity and an angular velocity to define a particle, and determine a scoring array including the particle, the velocity, and the angular velocity according to the map information and the positioning information and in combination with the local obstacle avoidance dynamic window algorithm;

[0095] The optimal mutual collision avoidance planner 20 is used to select the optimal particles in the feasible domain according to the score array and in combination with the optimal mutual collision avoidance algorithm.

[0096] The beneficial effect of this embodiment is that a particle is defined by taking a velocity and an angular velocity one by one through the local obstacle avoidance dynamic window planner 10, and a scoring array including the particle, the velocity, and the angular velocity is determined according to the map information and positioning information and in combination with the local obstacle avoidance dynamic window algorithm, and then the optimal mutual collision avoidance planner 20 selects the optimal particle in the feasible domain according to the scoring array and in combination with the optimal mutual collision avoidance algorithm. A high-performance obstacle avoidance planning scheme is realized, which solves the conversion problem of different velocity vectors, effectively avoids the excessive delta error caused by too large time interval dt, velocity v, and angular velocity w, and the collision problem that may arise, and while retaining the good multi-machine planning performance based on the ORCA algorithm, it also adapts to the characteristics of the differential drive robot.

[0097] Embodiment 5

[0098] The system also includes an instant positioning and map building module 30, a local cost map module 40, a global cost map module 50, a positioning information module 60 and a cloud storage module 70, wherein:

[0099] The local cost map module 40 is used to obtain local cost map data;

[0100] The global cost map module 50 is used to obtain global cost map data;

[0101] The positioning information module 60 is used to obtain positioning information data;

[0102] The cloud storage module 70 is used to read the position and speed data of other robots;

[0103] The real-time positioning and map building module 30 is used to provide the local cost map data, the global cost map data and the positioning information data to the local obstacle avoidance dynamic window planner 10;

[0104] The local obstacle avoidance dynamic window planner 10 is also used to generate the scoring array according to the local cost map data, the global cost map data and the positioning information data;

[0105] The optimal mutual collision avoidance planner 20 is further configured to output an optimal speed array according to the score array and the position speed data.

[0106] Specifically, refer to Figure 6 , which is a schematic diagram of the second module of the obstacle avoidance planning system provided in an embodiment of the present invention.

[0107] In this embodiment, first, the local obstacle avoidance dynamic window planner 10 reads the local cost map data, the global cost map data and the positioning information data provided by the real-time positioning and map construction module 30, and then generates an array containing speed v, angular velocity w and various scores of particles according to the above local cost map data, the global cost map data and the positioning information data. Then, the array of speed v, angular velocity w and various scores of particles is used as input items of the optimal mutual collision avoidance planner 20. At the same time, the optimal mutual collision avoidance planner 20 reads the position speed and other information of other robots in the cloud storage module 70. Finally, the optimal mutual collision avoidance planner 20 outputs an optimal speed array (v, w).

[0108] It should be noted that the above-mentioned system embodiment and method embodiment belong to the same concept, and the specific implementation process is detailed in the method embodiment, and the technical features in the method embodiment are correspondingly applicable in the device embodiment, which will not be repeated here.

[0109] Embodiment 6

[0110] This embodiment proposes an obstacle avoidance planning device, which includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the computer program is executed by the processor, the steps of the obstacle avoidance planning method described in any one of the above items are implemented.

[0111] It should be noted that the above-mentioned device embodiment and method embodiment belong to the same concept, and the specific implementation process is detailed in the method embodiment, and the technical features in the method embodiment are correspondingly applicable in the device embodiment, which will not be repeated here.

[0112] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the existence of other identical elements in the process, method, article or device including the element.

[0113] The serial numbers of the above embodiments of the present invention are only for description and do not represent the advantages or disadvantages of the embodiments.

[0114] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus a necessary general hardware platform, and of course by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product, which is stored in a storage medium (such as ROM / RAM, a magnetic disk, or an optical disk), and includes a number of instructions for enabling a terminal (which can be a mobile phone, a computer, a server, an air conditioner, or a network device, etc.) to execute the methods described in each embodiment of the present invention.

[0115] The embodiments of the present invention are described above in conjunction with the accompanying drawings, but the present invention is not limited to the above-mentioned specific implementation modes, which are merely illustrative rather than restrictive. Under the guidance of the present invention, ordinary technicians in this field can also make many forms without departing from the scope of protection of the present invention and the claims, all of which are within the protection of the present invention.

Claims

1. An obstacle avoidance planning method, It is characterized in that The method comprises: Define a particle by taking a velocity and angular velocity one by one; Determine a scoring array including the particle, the velocity, and the angular velocity according to the map information and the positioning information and in combination with a local obstacle avoidance dynamic window algorithm; According to the scoring array, the optimal particle is selected in the feasible domain in combination with the optimal mutual collision avoidance algorithm; The method of successively taking a velocity and an angular velocity to define a particle includes: In combination with the local obstacle avoidance dynamic window algorithm, determine a speed interval corresponding to the current speed and an angular velocity interval corresponding to the current angular velocity; In the speed interval and the angular velocity interval, a speed value and an angular velocity value are successively taken as the attribute of the particle at a preset time interval; The step of selecting the best particle in the feasible domain according to the score array and in combination with the best mutual collision avoidance algorithm includes: Expanding the obstacle avoidance radius of the robot according to the particle, the speed, and the angular velocity, and generating a plurality of boundary lines in combination with the optimal mutual collision avoidance algorithm; After the multiple boundary lines are generated by combining the optimal mutual collision avoidance algorithm, the method further comprises: Determine successively at the time interval whether the velocity array corresponding to the particle is within the boundary line; If the speed array is within the boundary line, determining whether the current path error is less than a preset error value; The determining whether the current path error is less than a preset error value includes: If the path error is smaller than a preset error value, the magnitude relationship between the index number of the particle and the preset primary selection number is determined.

2. The obstacle avoidance planning method according to claim 1, It is characterized in that The determining of the size relationship between the index number of the particle and the preset first-level selection number includes: If the index number is less than the first-level selection number, six loss items are calculated, wherein the six loss items include the Euclidean distance to the nearest global planning point, the heading angle difference to the nearest global planning point, the Euclidean distance to the preset point after the nearest global planning point, the heading angle difference to the preset point after the nearest global planning point, the distance to the nearest obstacle on the curve, and the penalty item of angular velocity; The six loss items are linearly weighted as the overall loss, and the overall loss is saved in the loss array.

3. The obstacle avoidance planning method according to claim 2, It is characterized in that The determining of the size relationship between the index number of the particle and the preset first-level selection number further includes: If the index number is equal to the first-level selection number, the speed array corresponding to the minimum overall loss is determined in the loss array, and the particle corresponding to the speed array is used as the optimal particle.

4. The obstacle avoidance planning method according to claim 3, It is characterized in that The determining of the size relationship between the index number of the particle and the preset first-level selection number further includes: If the index number is greater than the primary selection number and the index number is less than the preset secondary selection number, then determine a velocity array whose path error is less than a preset error value and is within the boundary line, and use the particle corresponding to the velocity array as the optimal particle; If the index number is greater than the secondary selection number, the speed array corresponding to the maximum score is determined according to the local obstacle avoidance dynamic window algorithm, and the particle corresponding to the speed array is used as the optimal particle.

5. An obstacle avoidance planning system, It is characterized in that The system comprises: A local obstacle avoidance dynamic window planner is used to successively take a velocity and an angular velocity to define a particle, and determine a scoring array including the particle, the velocity, and the angular velocity according to the map information and the positioning information and in combination with the local obstacle avoidance dynamic window algorithm; An optimal mutual collision avoidance planner, used to select optimal particles in a feasible domain according to the score array and in combination with an optimal mutual collision avoidance algorithm; The local obstacle avoidance dynamic window planner is specifically used to, in combination with the local obstacle avoidance dynamic window algorithm, determine a speed interval corresponding to a current speed and an angular velocity interval corresponding to a current angular velocity; within the speed interval and the angular velocity interval, successively take a speed value and an angular velocity value as the attribute of the particle at a preset time interval; expand the obstacle avoidance radius of the robot according to the particle, the speed and the angular velocity, and generate multiple boundary lines in combination with the optimal mutual collision avoidance algorithm; successively determine whether the speed array corresponding to the particle is within the boundary line at the time interval; if the speed array is within the boundary line, determine whether the current path error is less than a preset error value; if the path error is less than the preset error value, determine the size relationship between the index number of the particle and the preset first-level selection number.

6. The obstacle avoidance planning system according to claim 5, It is characterized in that The system also includes an instant positioning and map construction module, a local cost map module, a global cost map module, a positioning information module and a cloud storage module, wherein: The local cost map module is used to obtain local cost map data; The global cost map module is used to obtain global cost map data; The positioning information module is used to obtain positioning information data; The cloud storage module is used to read the position and speed data of other robots; The real-time positioning and map construction module is used to provide the local cost map data, the global cost map data and the positioning information data to the local obstacle avoidance dynamic window planner; The local obstacle avoidance dynamic window planner is further used to generate the scoring array according to the local cost map data, the global cost map data and the positioning information data; The optimal mutual collision avoidance planner is also used to output an optimal speed array based on the score array and the position speed data.

7. An obstacle avoidance planning device, It is characterized in that The device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the computer program implements the steps of the obstacle avoidance planning method as claimed in any one of claims 1 to 4 when executed by the processor.