Local path planning method, system and device integrating improved speed obstacle and TEB algorithm and medium

Abstract

The invention discloses a local path planning method, system and device fusing improved speed obstacle and a TEB algorithm and a medium, and the method comprises the steps: building a basic environment of local path planning, and obtaining a global static map, positioning information and a global path planning point; the method comprises the following steps: acquiring laser radar data, and realizing clustering and boundalization of obstacle information by using DBScan and a monotonic chain convex hull algorithm so as to construct a local cost map; adding linear velocity, adding angular velocity, curvature and velocity obstacle constraint are introduced to improve a TEB algorithm; and constructing an improved algorithm model, adjusting the weight coefficient of the speed obstacle constraint according to the environmental complexity, and solving the improved algorithm model. When the method is used for local path planning, a smooth and safe path can be output, and the calculation amount of processing a local cost map by a TEB algorithm is reduced; on the basis of considering the influence of the real environment, the speed obstacle and the TEB algorithm are fused and improved, and the obstacle avoidance capability of obstacles, especially dynamic obstacles, is improved.

Description

Technical Field

[0001] The present invention belongs to the field of local path planning of mobile robots, and in particular relates to a local path planning method, system, equipment and medium integrating improved speed barrier and TEB algorithm. Background Art

[0002] With the advancement and application of science and technology, mobile robots have been widely used in various industries and play an irreplaceable role in some application scenarios. Mobile robot technology has now become one of the important technologies in modern industry. The main research contents of mobile robot technology include positioning, mapping, and path planning. Path planning needs to realize two functions: global path finding and real-time obstacle avoidance, which are undertaken by global path planning and local path planning respectively. The quality of local path planning determines the ability of mobile robots to adapt to complex dynamic environments.

[0003] Local path planning technology plays a role in mobile robots avoiding known or unknown static or dynamic obstacles. Existing common local path planning technologies include: artificial potential field method, time elastic band method (TEB), Bug algorithm, dynamic window method, etc. Among them, the time elastic band method (TEB) has strong adaptability in simple dynamic environments, so it is widely used in actual mobile robot applications. However, the linear velocity and angular velocity of the TEB algorithm jump significantly in areas with dense obstacles, and it is weak in complex dynamic environments, so it needs to be improved.

[0004] Faced with complex dynamic environments and higher safety obstacle avoidance requirements, the existing TEB improvement method increases the impact of obstacles on the planned path, making the mobile robot more inclined to stay away from obstacles. This improvement method sacrifices the obstacle avoidance distance and time in exchange for the safety of obstacle avoidance, and the obstacle avoidance effect of dynamic obstacles is slightly improved. A further improvement method is to divide the obstacle influence area and handle it differently, reducing the sacrifice of obstacle avoidance distance and time in some cases, but there is still a problem of poor dynamic obstacle avoidance. Summary of the invention

[0005] In view of the deficiencies in the prior art, the present invention provides a local path planning method, system, device and medium that integrates improved speed obstacles and TEB algorithms. In view of the problem that the local path planning algorithm is not good at avoiding dynamic obstacles in complex environments, the speed obstacle and TEB algorithm are integrated and improved, and the acceleration constraint and curvature constraint are introduced to improve the speed jump and uneven path curve of the traditional TEB algorithm. The speed obstacle constraint is conditionally introduced to improve the dynamic obstacle avoidance capability of the TEB algorithm. At the same time, DBScan and monotone chain convex hull algorithm are used to realize the clustering and boundary of obstacle information when constructing the local cost map, reducing the calculation amount of the TEB algorithm and thus reducing the response time of the algorithm.

[0006] To achieve the above object, the present invention adopts the following technical solutions:

[0007] A local path planning method integrating improved speed barrier and TEB algorithm comprises the following steps:

[0008] S1, build the basic environment of local path planning, including obtaining the global static map, the robot's positioning information and the global path planning points; based on the global path planning points, use the global path planning algorithm to calculate the global planning path, and split the global planning path into several path points as local path planning points;

[0009] S2, using laser radar to obtain point cloud data of the environment, using DBScan algorithm to cluster the point cloud data to obtain obstacle clusters, using monotone chain convex hull algorithm to perform boundary processing on obstacle clusters, and constructing local cost maps; the global static map and local cost maps constitute the actual obstacle avoidance cost map;

[0010] S3, on the actual obstacle avoidance cost map, introduce the linear velocity constraint, angular velocity constraint, local path curvature constraint and speed obstacle constraint to improve the TEB algorithm; construct the target total cost function of the TEB algorithm;

[0011] S4, judge the complexity of the current local environment of the mobile robot, adjust the weight coefficient of the speed obstacle constraint according to the complexity of the environment, solve the target total cost function of the TEB algorithm, obtain the parameter set of the optimized robot trajectory, convert the parameter set into continuous linear speed and angular velocity to control the movement of the robot chassis, and navigate in sequence using the local path planning points as target points to achieve obstacle avoidance in the actual obstacle avoidance cost map.

[0012] To optimize the above technical solutions, the specific measures taken also include:

[0013] Furthermore, in S1, the method for acquiring the positioning information of the robot is to use GPS.

[0014] Furthermore, in S1, the method for acquiring the positioning information of the robot is to use an odometer.

[0015] Furthermore, in S1, the method for acquiring the positioning information of the robot is to use the amcl algorithm.

[0016] Furthermore, in S2, the point cloud data is clustered using the DBScan algorithm as follows:

[0017] The domain definition formula for a single point in the LiDAR point cloud data is:

[0018] N ∈(p)={q∈D|d(p,q)≤∈}

[0019] Among them, N ∈ (p) is the neighborhood of a single point, p and q are any two points in the original lidar point cloud data, D is the point set, d(p,q) is the distance between the two points, and ∈ is the radius defining the neighborhood range;

[0020] The core point condition formula is:

[0021] |N ∈ (p)|≥minPts

[0022] Among them, minPts represents the number of adjacent points that a point p has at least in the area where it is judged as a core point. If a point p meets the above core point condition formula, it is judged as a core point;

[0023] The direct density reachability condition formula is:

[0024] q∈N ∈ (p)and|N ∈ (p)|≥minPts

[0025] If a point q satisfies the above direct density reachability condition formula, it is determined to be a direct density reachable point;

[0026] All points are divided into core points, directly density reachable points and noise points. P is the set of all core points, Q is the set of all directly density reachable points, and the remaining points are noise points. The core points in P and the directly density reachable points in Q form an obstacle cluster.

[0027] Furthermore, in S2, the use of the monotone chain convex hull algorithm to perform boundary processing on the obstacle cluster is specifically as follows:

[0028] The static obstacle clusters are converted into point sets whose outer contours are polygons or line segments, and the dynamic obstacle clusters are converted into point sets whose outer contours are line segments.

[0029] Further, in S3, the calculation formula of the Jiajia linear velocity and the Jiajia angular velocity is:

[0030]

[0031] In the formula, is the added linear speed, is the angular velocity of the i is the average linear acceleration of the mobile robot from the i-th posture to the i+1-th posture, ω i is the average angular acceleration of the mobile robot from the i-th posture to the i+1-th posture, ΔT i The time it takes to move the robot from the i-th posture to the i+1-th posture.

[0032] Furthermore, in S3, the curvature constraint of the local path is specifically:

[0033] The local path is represented by a curve of y=f(x), and the curvature K is calculated as:

[0034]

[0035] Here, curvature is the limit of the angle differential dθ and the arc length differential dl, where l is the arc length of the path, are the first and second derivatives of the curve y, Δθ is the angle change, and Δl is the arc length change.

[0036] Furthermore, the speed obstacle constraint is introduced as follows:

[0037] Get dynamic obstacle information, combine the radius and speed of the dynamic obstacle with the mobile robot, and use the speed barrier method to calculate the collision cone between the mobile robot and the obstacle. The collision cone is the relative speed set of the robot and a single dynamic obstacle. Multiple collision cones form a speed barrier. The speed barrier constraint is introduced into the TEB algorithm to limit the speed vector output by the TEB algorithm to the outside of the speed barrier to achieve obstacle avoidance.

[0038] The speed barrier calculation formula is expressed as:

[0039]

[0040] Among them, V A is the speed of the mobile robot, P A is the position of the mobile robot, V B is the speed of the dynamic obstacle, is the relative speed obstacle area between dynamic obstacle B and mobile robot A, λ(P A ,V A -V B ) is the ray emitted from the geometric center of robot A along the direction of the relative speed between the robot and the dynamic obstacle; φ represents the empty set;

[0041] The speed obstacle constraint introduced into the TEB algorithm is specifically:

[0042] Add a maximum uncertainty radius ΔR affected by the minimum decision time to the dynamic obstacle threat radius in the speed obstacle method B , so that the calculated speed obstacle gets a dynamic expansion effect, leaving room for the robot to react; the new dynamic obstacle threat radius R Q The calculation formula is as follows:

[0043] R Q =R A +RB +d*ΔR B

[0044]

[0045] Among them, R A is the radius of the mobile robot, R B is the radius of the dynamic obstacle, d is a constant, and is adjusted according to the speed of the surrounding dynamic obstacles. B is the maximum uncertainty radius, Δt is the minimum decision time, V B (t) is the velocity of the dynamic obstacle at time t, ΔV B is the maximum rate change estimate of the dynamic obstacle, Δθ B is the estimated maximum direction change of the dynamic obstacle.

[0046] Furthermore, in S3, the target total cost function of the TEB algorithm is specifically:

[0047]

[0048] Among them, f(X) is the total cost of the target, X is the parameter set of the robot trajectory, and f k (X) is the kth constraint objective function, γ k is the weight coefficient of the kth constraint objective function; f nh (X) is the nonholonomic kinematic constraint, γ nh represents the weight coefficient of the nonholonomic kinematic constraint; f time (X) is the time optimal constraint, γ time represents the weight coefficient of the time optimal constraint; f v (X) is the velocity constraint, γ v Represents the weight coefficient of speed constraint; f a (X) is the acceleration constraint, γ a represents the weight coefficient of acceleration constraint; f path (X) is the path constraint, γ path represents the weight coefficient of the path constraint; f ob (X) is the obstacle constraint, γ ob represents the weight coefficient of obstacle constraint; f jerk (X) is the jerk constraint, γ jerk represents the jerk constraint weight coefficient; f curv (X) is the curvature constraint, γ curv represents the curvature constraint weight coefficient; f vo (X) is the speed obstacle constraint, γ vo Represents the speed barrier constraint weight coefficient.

[0049] Furthermore, in S4, the complexity of the current local environment of the mobile robot is judged, and the weight coefficient of the speed obstacle constraint is adjusted according to the complexity of the environment:

[0050] Number of dynamic obstacles N B and the dynamic obstacle velocity V B If at least one parameter reaches the set threshold, the current environment is judged as a complex environment; when the number of static obstacles around reaches the set value and there are dynamic obstacles, the current environment is also judged as a complex environment, and other situations are non-complex environments;

[0051] When the environment is not judged as a complex environment, the speed obstacle constraint weight coefficient γ vo Set to zero.

[0052] Further, in S4, the method used for solving the target total cost function of the TEB algorithm is to solve it based on the g2o framework;

[0053] The optimization calculation formula is as follows:

[0054] X * =arg X minf(X)

[0055] Among them, X * is the optimization result of the TEB algorithm, which represents a parameter set of an optimized robot trajectory, including several position points and the interval time information between each two adjacent position points. Each position point includes posture information and position information; arg X minf(X) represents the calculation of the parameter X that minimizes the target total cost function f(X).

[0056] The present invention also proposes a local path planning system integrating the improved speed barrier and the TEB algorithm, comprising:

[0057] The environment building module is used to build the basic environment for local path planning, including obtaining the global static map, the robot's positioning information and the global path planning points; based on the global path planning points, the global path planning algorithm is used to calculate the global planning path, and the global planning path is split into several path points as local path planning points;

[0058] The cost map generation module uses the DBScan algorithm to cluster the point cloud data of the environment acquired by the lidar to obtain obstacle clusters, and uses the monotone chain convex hull algorithm to perform boundary processing on the obstacle clusters to construct a local cost map; the global static map and the local cost map constitute the actual obstacle avoidance cost map;

[0059] The TEB algorithm model building module is used to improve the TEB algorithm by introducing the linear velocity constraint, angular velocity constraint, local path curvature constraint and speed obstacle constraint on the actual obstacle avoidance cost map; and to build the target total cost function of the TEB algorithm;

[0060] The solution module is used to judge the complexity of the current local environment of the mobile robot, adjust the weight coefficient of the speed obstacle constraint according to the complexity of the environment, solve the target total cost function of the TEB algorithm, and obtain the parameter set of the optimized robot trajectory;

[0061] The navigation module is used to convert the parameter set into continuous linear velocity and angular velocity to control the movement of the robot chassis, and navigate the local path planning points as target points in sequence to achieve obstacle avoidance in the actual obstacle avoidance cost map.

[0062] To optimize the above technical solutions, the specific measures taken also include:

[0063] Furthermore, the system also includes a GPS for obtaining positioning information of the robot.

[0064] Furthermore, the system also includes an odometer for obtaining positioning information of the robot.

[0065] Furthermore, the method for acquiring the positioning information of the robot is to use the amcl algorithm.

[0066] The present invention also proposes an electronic device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the local path planning method integrating the improved speed barrier and the TEB algorithm as described above is implemented.

[0067] The present invention also provides a computer-readable storage medium storing a computer program, wherein the computer program enables a computer to execute the local path planning method integrating the improved speed barrier and the TEB algorithm as described above.

[0068] The beneficial effects of the present invention are:

[0069] The present invention integrates and improves the speed obstacle and TEB algorithm, introduces jerk constraints and curvature constraints to improve the speed jump and non-smooth path curve problems of the traditional TEB algorithm, conditionally introduces speed obstacle constraints to improve the dynamic obstacle avoidance capability of the TEB algorithm, introduces speed obstacle constraints and uses DBScan and monotone chain convex hull algorithms to cluster and boundary obstacle information when constructing a local cost map, reduces the amount of calculation of the TEB algorithm, and thus reduces the response time of the algorithm. The present invention can output a smooth and safe path for local path planning, reduces the amount of calculation of the TEB algorithm for processing local cost maps; integrates and improves the speed obstacle and TEB algorithm on the basis of considering the impact of the real environment, and improves the obstacle avoidance capability of obstacles, especially dynamic obstacles. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] Figure 1 A schematic flow chart of a local path planning method integrating improved speed barrier and TEB algorithm according to an embodiment of the present invention;

[0071] Figure 2 A hypergraph of kinematic constraints, velocity constraints, acceleration constraints, and obstacle constraints in the TEB algorithm according to an embodiment of the present invention;

[0072] Figure 3 is a hypergraph of a jerk constraint according to an embodiment of the present invention;

[0073] Figure 4 is a hypergraph of speed barrier constraints according to an embodiment of the present invention. DETAILED DESCRIPTION

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

[0075] Embodiment 1

[0076] The present invention proposes a local path planning method that integrates improved speed barriers and the TEB algorithm. The local path planning method can output a smooth and safe path, reducing the amount of calculation of the TEB algorithm in processing the local cost map. The TEB algorithm is improved on the basis of considering the impact of the real environment, and its obstacle avoidance ability, especially dynamic obstacles, is enhanced. The flowchart of the method is as follows Figure 1 As shown, the following steps are included:

[0077] S1, build the basic environment for local path planning, including obtaining the global static map for reading known static obstacle information; obtaining the robot's positioning information, optionally using GPS, odometer, amcl positioning and other methods to determine the exact position of the mobile robot in the map; obtaining the global path planning points; based on the global path planning points, using the global path planning algorithm to calculate the global planning path, and split the global planning path into several path points as local path planning points.

[0078] S2, using LiDAR to obtain point cloud data of the environment. Since the ultimate improvement goal is to enable the mobile robot to adapt to and safely avoid obstacles in complex environments, the obstacle information displayed by the original LiDAR data is too complex and noisy, which is not conducive to the calculation of the algorithm and thus affects its reaction speed. Therefore, the DBScan algorithm is used to cluster the point cloud data to obtain obstacle clusters marked as different obstacles. The monotone chain convex hull (MCCH) algorithm is used to perform boundary processing on the obstacle clusters, and finally the obstacle information that filters out certain noise and is easy to calculate is obtained to construct a local cost map; the global static map and the local cost map constitute the actual obstacle avoidance cost map.

[0079] In S2, the point cloud data is clustered using the DBScan algorithm as follows:

[0080] The domain definition formula for a single point in the LiDAR point cloud data is:

[0081] N ∈ (p)={q∈D|d(p,q)≤∈}

[0082] Among them, N ∈ (p) is the neighborhood of a single point, p and q are any two points in the original lidar point cloud data, D is the point set, d(p,q) is the distance between the two points, and ∈ is the radius defining the neighborhood range;

[0083] The core point condition formula is:

[0084] |N ∈ (p)|≥minPts

[0085] Among them, minPts represents the number of adjacent points that a point p has at least in the area where it is judged as a core point. If a point p meets the above core point condition formula, it is judged as a core point;

[0086] The direct density reachability condition formula is:

[0087] q∈N ∈ (p)and|N ∈ (p)|≥minPts

[0088] If a point q satisfies the above direct density reachability condition formula, it is determined to be a direct density reachable point;

[0089] All points are divided into core points, directly density reachable points and noise points. P is the set of all core points, Q is the set of all directly density reachable points, and the remaining points are noise points. The core points in P and the directly density reachable points in Q form an obstacle cluster.

[0090] In S2, the boundary processing of the obstacle cluster using the monotone chain convex hull algorithm is specifically as follows:

[0091] The static obstacle clusters are converted into point sets whose outer contours are polygons or line segments, and the dynamic obstacle clusters are converted into point sets whose outer contours are line segments.

[0092] S3, on the actual obstacle avoidance cost map, linear velocity constraints, angular velocity constraints, local path curvature constraints and speed obstacle constraints are introduced to improve the TEB algorithm; the target total cost function of the TEB algorithm is constructed; adding linear velocity constraints, angular velocity constraints and local path curvature constraints can improve the speed jump and path smoothness problems of the traditional TEB algorithm.

[0093] The calculation formula of the Jiajia linear velocity and Jiajia angular velocity is:

[0094]

[0095] In the formula, is the added linear speed, is the angular velocity of the i is the average linear acceleration of the mobile robot from the i-th posture to the i+1-th posture, ω i is the average angular acceleration of the mobile robot from the i-th posture to the i+1-th posture, ΔT i The time it takes to move the robot from the i-th posture to the i+1-th posture.

[0096] The curvature constraint of the local path is specifically:

[0097] The local path is represented by a curve of y=f(x), and the curvature K is calculated as:

[0098]

[0099] Here, curvature is the limit of the angle differential dθ and the arc length differential dl, where l is the arc length of the path, are the first and second derivatives of the curve y, Δθ is the angle change, and Δl is the arc length change.

[0100] The speed barrier constraint is introduced as follows:

[0101] Get dynamic obstacle information, combine the radius and speed of the dynamic obstacle with the mobile robot, and use the speed barrier method to calculate the collision cone between the mobile robot and the obstacle. The collision cone is the relative speed set of the robot and a single dynamic obstacle. Multiple collision cones form a speed barrier. The speed barrier constraint is introduced into the TEB algorithm to limit the speed vector output by the TEB algorithm to the outside of the speed barrier to achieve obstacle avoidance.

[0102] The speed barrier calculation formula is expressed as:

[0103]

[0104] Among them, V A is the speed of the mobile robot, P A is the position of the mobile robot, V B is the speed of the dynamic obstacle, is the relative speed obstacle area between dynamic obstacle B and mobile robot A, λ(P A ,V A -V B ) is the ray emitted from the geometric center of robot A along the direction of the relative speed between the robot and the dynamic obstacle; φ represents the empty set;

[0105] The speed obstacle constraint introduced into the TEB algorithm is specifically:

[0106] If the speed of the robot selected by the TEB algorithm falls outside the speed obstacle, then the obstacle can be avoided successfully in theory. Considering that the speed selected by the improved TEB algorithm may fall on the edge of the speed obstacle, and there is a possibility of sudden changes in the speed of dynamic obstacles in real scenarios, there is a risk of collision. Therefore, a maximum uncertainty radius ΔR affected by the minimum decision time is added to the dynamic obstacle threat radius in the speed obstacle method. B , so that the calculated speed obstacle obtains a dynamic expansion effect, which reserves the reaction distance for the robot; it reduces the possibility of collision during actual movement after the improved TEB algorithm selects the speed of the outer boundary of the speed obstacle. The new dynamic obstacle threat radius R Q The calculation formula is as follows:

[0107] R Q =R A +R B +d*ΔR B

[0108]

[0109] Among them, R A is the radius of the mobile robot, R B is the radius of the dynamic obstacle, d is a constant, and is adjusted according to the speed of the surrounding dynamic obstacles. Bis the maximum uncertainty radius, Δt is the minimum decision time, V B (t) is the velocity of the dynamic obstacle at time t, ΔV B is the maximum rate change estimate of the dynamic obstacle, Δθ B is the estimated maximum direction change of the dynamic obstacle.

[0110] The target total cost function of the TEB algorithm is specifically:

[0111]

[0112] Among them, f(X) is the total cost of the target, X is the parameter set of the robot trajectory, and f k (X) is the kth constraint objective function, γ k is the weight coefficient of the kth constraint objective function; f nh (X) is the nonholonomic kinematic constraint, γ nh represents the weight coefficient of the nonholonomic kinematic constraint; f time (X) is the time optimal constraint, γ time represents the weight coefficient of the time optimal constraint; f v (X) is the velocity constraint, γ v Represents the weight coefficient of speed constraint; f a (X) is the acceleration constraint, γ a represents the weight coefficient of acceleration constraint; f path (X) is the path constraint, γ path represents the weight coefficient of the path constraint; f ob (X) is the obstacle constraint, γ ob represents the weight coefficient of obstacle constraint; f jerk (X) is the jerk constraint, γ jerk represents the jerk constraint weight coefficient; f curv (X) is the curvature constraint, γ curv represents the curvature constraint weight coefficient; f vo (X) is the speed obstacle constraint, γ vo Represents the speed barrier constraint weight coefficient.

[0113] S4, judge the complexity of the current local environment of the mobile robot, adjust the weight coefficient of the speed obstacle constraint according to the complexity of the environment, solve the target total cost function of the TEB algorithm, obtain the parameter set of the optimized robot trajectory, convert the parameter set into continuous linear speed and angular velocity to control the movement of the robot chassis, and navigate in sequence using the local path planning points as target points to achieve obstacle avoidance in the actual obstacle avoidance cost map.

[0114] The complexity of the current local environment of the mobile robot is judged, and the weight coefficient of the speed obstacle constraint is adjusted according to the complexity of the environment:

[0115] Number of dynamic obstacles N B and the dynamic obstacle velocity V B If at least one parameter reaches the set threshold, the current environment is judged as a complex environment; when the number of static obstacles around reaches the set value and there are dynamic obstacles, the current environment is also judged as a complex environment, and other situations are non-complex environments;

[0116] When the environment is not judged as a complex environment, the speed obstacle constraint weight coefficient γ vo Set it to zero to reduce the amount of calculation and increase the calculation speed of the algorithm.

[0117] The method used to solve the target total cost function of the TEB algorithm is to solve it based on the g2o framework;

[0118] The optimization calculation formula is as follows:

[0119] X * =arg X minf(X)

[0120] Among them, X * is the optimization result of the TEB algorithm, which represents a parameter set of an optimized robot trajectory, including several position points and the interval time information between each two adjacent position points. Each position point includes posture information and position information; arg X minf(X) represents the calculation of the parameter X that minimizes the target total cost function f(X).

[0121] The solution of the TEB algorithm model is only related to several continuous states. The optimization problem can be transformed into a hypergraph problem and solved using g2o general graph optimization. Figure 2 A hypergraph of kinematic constraints, velocity constraints, acceleration constraints, and obstacle constraints in the TEB algorithm according to an embodiment of the present invention; Figure 3 is a hypergraph of a jerk constraint according to an embodiment of the present invention; Figure 4 is a hypergraph of speed barrier constraints according to an embodiment of the present invention.

[0122] Embodiment 2

[0123] The present invention proposes a local path planning system corresponding to the method of the first embodiment, which integrates the improved speed barrier and the TEB algorithm, including:

[0124] The environment building module is used to build the basic environment for local path planning, including obtaining the global static map, the robot's positioning information and the global path planning points; based on the global path planning points, the global planning path is calculated using the global path planning algorithm, and the global planning path is split into several path points as local path planning points; GPS, odometer, amcl positioning and other methods are optionally used to determine the exact position of the mobile robot in the map.

[0125] The cost map generation module uses the DBScan algorithm to cluster the point cloud data of the environment acquired by the lidar to obtain obstacle clusters, and uses the monotone chain convex hull algorithm to perform boundary processing on the obstacle clusters to construct a local cost map; the global static map and the local cost map constitute the actual obstacle avoidance cost map;

[0126] The TEB algorithm model building module is used to improve the TEB algorithm by introducing the linear velocity constraint, angular velocity constraint, local path curvature constraint and speed obstacle constraint on the actual obstacle avoidance cost map; and to build the target total cost function of the TEB algorithm;

[0127] The solution module is used to judge the complexity of the current local environment of the mobile robot, adjust the weight coefficient of the speed obstacle constraint according to the complexity of the environment, solve the target total cost function of the TEB algorithm, and obtain the parameter set of the optimized robot trajectory;

[0128] The navigation module is used to convert the parameter set into continuous linear velocity and angular velocity to control the movement of the robot chassis, and navigate the local path planning points as target points in sequence to achieve obstacle avoidance in the actual obstacle avoidance cost map.

[0129] The implementation methods of each module and module function in the system are completely consistent with the steps of the method in Example 1, so they will not be repeated here.

[0130] Embodiment 3

[0131] The present invention proposes an electronic device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the local path planning method integrating the improved speed barrier and the TEB algorithm as described in the first embodiment is implemented.

[0132] Embodiment 4

[0133] The present invention provides a computer-readable storage medium storing a computer program, wherein the computer program enables a computer to execute the local path planning method integrating the improved speed barrier and the TEB algorithm as described in the first embodiment.

[0134] In the embodiments disclosed in the present application, the computer storage medium can be a tangible medium that can contain or store a program for use by or in conjunction with an instruction execution system, device or equipment. The computer storage medium can include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices or equipment, or any suitable combination of the above. More specific examples of computer storage media can include electrical connections based on one or more lines, portable computer disks, hard disks, random access memories (RAM), read-only memories (ROM), erasable programmable read-only memories (EPROM or flash memory), optical fibers, portable compact disk read-only memories (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the above.

[0135] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed in this application can be implemented in electronic hardware or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of this application.

[0136] The above are only preferred embodiments of the present invention, and the protection scope of the present invention is not limited to the above embodiments. All technical solutions under the concept of the present invention belong to the protection scope of the present invention. It should be pointed out that for ordinary technicians in this technical field, some improvements and modifications without departing from the principle of the present invention should be regarded as the protection scope of the present invention.

Claims

1. A local path planning method integrating improved speed barrier and TEB algorithm, characterized in that: The following steps are involved: S1, build the basic environment for local path planning, including obtaining the global static map, the robot's positioning information and the global path planning points; Based on the global path planning points, the global planning path is calculated using the global path planning algorithm, and the global planning path is split into several path points as local path planning points; S2 uses LiDAR to obtain point cloud data of the environment, clusters the point cloud data using the DBScan algorithm to obtain obstacle clusters, and uses the monotone chain convex hull algorithm to perform boundary processing on the obstacle clusters to construct a local cost map; the global static map and the local cost map form the actual obstacle avoidance cost map; S3, on the actual obstacle avoidance cost map, introduce the linear velocity constraint, angular velocity constraint, local path curvature constraint and speed obstacle constraint to improve the TEB algorithm; construct the target total cost function of the TEB algorithm; S4, judge the complexity of the current local environment of the mobile robot, adjust the weight coefficient of the speed obstacle constraint according to the complexity of the environment, solve the target total cost function of the TEB algorithm, obtain the parameter set of the optimized robot trajectory, convert the parameter set into continuous linear velocity and angular velocity to control the movement of the robot chassis, and use the local path planning points as target points for navigation in sequence to achieve obstacle avoidance in the actual obstacle avoidance cost map.

2. The local path planning method integrating the improved speed barrier and TEB algorithm as claimed in claim 1, characterized in that: In S1, the method for obtaining the positioning information of the robot is to use GPS.

3. The local path planning method integrating the improved speed barrier and TEB algorithm as claimed in claim 1, characterized in that: In S1, the method for obtaining the positioning information of the robot is to use an odometer.

4. The local path planning method integrating the improved speed barrier and TEB algorithm as claimed in claim 1, characterized in that: In S1, the method for obtaining the positioning information of the robot is to use the amcl algorithm.

5. The local path planning method integrating the improved speed barrier and TEB algorithm as claimed in claim 1, characterized in that: In S2, the point cloud data is clustered using the DBScan algorithm as follows: The domain definition formula for a single point in the lidar point cloud data is: N ∈ (p)={q∈D∣d(p,q)≤∈} Among them, N ∈ (p) is the neighborhood of a single point, p and q are any two points in the original lidar point cloud data, D is the point set, d(p,q) is the distance between the two points, and ∈ is the radius defining the neighborhood range; The core point condition formula is: |N ∈ (p)|≥minPts Among them, minPts represents the minimum number of adjacent points that a point p has in the area where it is judged as a core point. If a point p meets the above core point condition formula, it is determined to be a core point; The direct density reachability condition formula is: q∈N ∈ (p)and|N ∈ (p)|≥minPts If a point q satisfies the above direct density reachability condition formula, it is determined to be a direct density reachable point; All points are divided into core points, directly density-reachable points, and noise points. P is the set of all core points, Q is the set of all directly density-reachable points, and the remaining points are noise points. The core points in P and the directly density-reachable points in W form an obstacle cluster.

6. The local path planning method integrating the improved speed barrier and TEB algorithm as claimed in claim 1, characterized in that: In S2, the boundary processing of the obstacle cluster using the monotone chain convex hull algorithm is specifically as follows: The static obstacle cluster is converted into a point set whose outer contour is a polygon or line segment, and the dynamic obstacle cluster is converted into a point set whose outer contour is a line segment.

7. The local path planning method integrating the improved speed barrier and TEB algorithm as claimed in claim 1, characterized in that: In S3, the calculation formulas of the Jiajia linear velocity and the Jiajia angular velocity are: Where, is the linear velocity, is the angular velocity of the i is the average linear acceleration of the mobile robot from the i-th posture to the i+1-th posture, ω i is the average angular acceleration of the mobile robot from the i-th posture to the i+1-th posture, ΔT i The time it takes to move the robot from the i-th posture to the i+1-th posture.

8. The local path planning method integrating the improved speed barrier and TEB algorithm as claimed in claim 1, characterized in that: In S3, the curvature constraint of the local path is specifically: The local path is represented by a curve, and the curvature K is calculated as follows: Here, curvature is the limit of the angle differential dθ and the arc length differential dl, where l is the arc length of the path, are the first and second derivatives of the curve, Δθ is the change in angle, and Δl is the change in arc length.

9. The local path planning method integrating the improved speed barrier and TEB algorithm as claimed in claim 1, characterized in that: The speed barrier constraint is introduced as follows: Obtain dynamic obstacle information. Combined with the radius and speed of the dynamic obstacle and the mobile robot, the speed barrier method is used to calculate the collision cone between the mobile robot and the obstacle. The collision cone is the set of relative velocities at which a collision occurs between the robot and a single dynamic obstacle. Multiple collision cones form a speed barrier. A speed barrier constraint is introduced into the TEB algorithm to limit the velocity vector output by the TEB algorithm to within the speed barrier to achieve obstacle avoidance. The speed barrier calculation formula is expressed as: Among them, V A is the speed of the mobile robot, P A is the position of the mobile robot, V B is the velocity of the dynamic obstacle, is the relative speed obstacle area between the dynamic obstacle B and the mobile robot A, λ(P A ,V A -V B ) is the ray emitted from the geometric center of robot A along the direction of the relative velocity between the robot and the dynamic obstacle; φ represents the empty set; The speed barrier constraint introduced into the TEB algorithm is specifically as follows: Add a maximum uncertainty radius ΔR to the dynamic obstacle threat radius in the speed barrier method, which is affected by the minimum decision time. B , so that the calculated speed obstacle gets a dynamic expansion effect, which reserves the reaction distance for the robot; the new dynamic obstacle threat radius R Q The calculation formula is as follows: R Q =R A +R B +d*ΔR B Among them, R A is the radius of the mobile robot, R B is the radius of the dynamic obstacle, d is a constant, and is adjusted according to the speed of the surrounding dynamic obstacles, ΔR B is the maximum uncertainty radius, Δt is the minimum decision time, V B (t) is the velocity of the dynamic obstacle at time t, ΔV B is the maximum rate change estimate of the dynamic obstacle, Δθ B is the estimated maximum direction change of the dynamic obstacle.

10. The local path planning method integrating the improved speed barrier and TEB algorithm according to claim 1, characterized in that: In S3, the target total cost function of the TEB algorithm is specifically: Among them, f(X) is the total cost of the target, X is the parameter set of the robot trajectory, and f k (X) is the k-th constraint objective function, γ k is the weight coefficient of the kth constraint objective function; f nh (X) is the nonholonomic kinematic constraint, γ nh represents the weight coefficient of the nonholonomic kinematic constraint; f time (X) is the time optimal constraint, γ time represents the weight coefficient of the time optimal constraint; f v (X) is the velocity constraint, γ v represents the weight coefficient of speed constraint; f a (X) is the acceleration constraint, γ a represents the weight coefficient of acceleration constraint; f path (X) is the path constraint, γ path represents the weight coefficient of the path constraint; f ob (X) is the obstacle constraint, γ ob represents the weight coefficient of obstacle constraint; f jerk (X) is the jerk constraint, γ jerk represents the acceleration constraint weight coefficient; f curv (X) is the curvature constraint, γ curv represents the curvature constraint weight coefficient; f vo (X) is the speed obstacle constraint, γ vo Represents the speed barrier constraint weight coefficient.

11. The local path planning method integrating the improved speed barrier and the TEB algorithm according to claim 10, characterized in that: In S4, the complexity of the current local environment of the mobile robot is judged, and the weight coefficient of the speed obstacle constraint is adjusted according to the complexity of the environment. Specifically, Number of dynamic obstacles N B and the dynamic obstacle velocity V B If at least one parameter reaches the set threshold, the current environment is judged to be a complex environment; when the number of static obstacles in the surrounding reaches the set value and there are dynamic obstacles, the current environment is also judged to be a complex environment, and other cases are non-complex environments; When the environment is not judged as a complex environment, the speed obstacle constraint weight coefficient γ vo Set to zero.

12. The local path planning method integrating the improved speed barrier and TEB algorithm according to claim 10, characterized in that: In S4, the method used to solve the target total cost function of the TEB algorithm is based on the g2o framework; the optimization calculation formula is as follows: X * =arg X minf(X) Among them, X * is the optimization result of the TEB algorithm, which represents a parameter set of the optimized robot trajectory, including several position points and the interval time information between each two adjacent position points. Each position point includes posture information and position information; arg X minf(X) represents the calculation of the parameter X that minimizes the target total cost function f(X).

13. A local path planning system integrating improved speed barrier and TEB algorithm, characterized in that: include: The environment building module is used to build the basic environment for local path planning, including obtaining the global static map, the robot's positioning information and the global path planning points; Based on the global path planning points, the global planning path is calculated using the global path planning algorithm, and the global planning path is split into several path points as local path planning points; The cost map generation module uses the DBScan algorithm to cluster the point cloud data of the environment acquired by the lidar to obtain obstacle clusters. The monotone chain convex hull algorithm is used to demarcate the obstacle clusters and construct a local cost map. The global static map and the local cost map form the actual obstacle avoidance cost map. The TEB algorithm model construction module is used to improve the TEB algorithm by introducing linear velocity constraints, angular velocity constraints, local path curvature constraints, and speed obstacle constraints on the actual obstacle avoidance cost map; and to construct the target total cost function of the TEB algorithm; The solution module is used to determine the complexity of the current local environment of the mobile robot, adjust the weight coefficient of the speed obstacle constraint according to the complexity of the environment, solve the target total cost function of the TEB algorithm, and obtain the parameter set of the optimized robot trajectory; The navigation module is used to convert the parameter set into continuous linear velocity and angular velocity to control the movement of the robot chassis, and use the local path planning points as target points for navigation in sequence to enable the robot to avoid obstacles in the actual obstacle avoidance cost map.

14. The local path planning system integrating the improved speed barrier and TEB algorithm according to claim 13, characterized in that: The system also includes a GPS for obtaining positioning information of the robot.

15. The local path planning system integrating the improved speed barrier and TEB algorithm according to claim 13, characterized in that: The system also includes an odometer for obtaining positioning information of the robot.

16. The local path planning system integrating the improved speed barrier and TEB algorithm according to claim 13, characterized in that: The method for obtaining the positioning information of the robot is to use the amcl algorithm.

17. An electronic device, characterized in that: include: A memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the local path planning method integrating the improved speed barrier and the TEB algorithm as described in any one of claims 1 to 12 is implemented.

18. A computer-readable storage medium storing a computer program, characterized in that: The computer program enables a computer to execute the local path planning method integrating the improved speed barrier and the TEB algorithm as described in any one of claims 1 to 12.

Images